diff --git a/CMakeLists.txt b/CMakeLists.txt
index ad25845f..f4b534a3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -66,7 +66,10 @@ endif(NOT (CMAKE_SIZEOF_VOID_P EQUAL 8) )
 
 find_package( OpenCV 2.4 COMPONENTS core highgui imgproc calib3d)
 IF (OpenCV_FOUND)
+  message("OpenCV_DIRS: ${OpenCV_INCLUDE_DIRS}")
+  message("OpenCV_LIBS: ${OpenCV_LIBS}")
   include_directories( ${OpenCV_INCLUDE_DIRS} )
+  include_directories( /usr/local/include/opencv2)
   set(EST_LIBS ${EST_LIBS} ${OpenCV_LIBS})
 ENDIF (OpenCV_FOUND)
 
diff --git a/bin/airbottest.cc b/bin/airbottest.cc
index 4644cc10..9dd76c97 100644
--- a/bin/airbottest.cc
+++ b/bin/airbottest.cc
@@ -158,12 +158,14 @@ void solver_process(Viewer* viewer)
     // getchar();
   }
 
-  int Nd;
+  /*
+  int Nd = 16;
   params.GetInt("Nd", Nd);
   vector<MbsState> xds(Nd+1, x0);  
   int d = N/Nd;
   for (int i=Nd, j = N; i>=0 && j>= 0; --i, j-=d)
     xds[i] = xs[j];
+  */
 
   //  xs = xds;
   
diff --git a/bin/body3dcedemstab.cc b/bin/body3dcedemstab.cc
index 20913098..0e07240b 100644
--- a/bin/body3dcedemstab.cc
+++ b/bin/body3dcedemstab.cc
@@ -36,7 +36,7 @@ class Body3dSampler : public Creator<Body3dState> {
     x = xf;
     for (int i = 0; i < N; ++i) {
       do {
-        // x.second[1] = 2*xf.second[1]*RND;
+        x.p(1) = 2*xf.p(1)*RND;
         if (con)
           (*con)(g, 0, x);
       } while (con && g[0] > -1); // 1 meter away from obstacles
diff --git a/bin/qrotor.cfg b/bin/qrotor.cfg
index 70273969..6bb50b18 100644
--- a/bin/qrotor.cfg
+++ b/bin/qrotor.cfg
@@ -1,11 +1,12 @@
 T=2
-N=36
+N=128
 
 x0 = 0,0,0, 0,0,0, 0,0,0, 0,0,0
-xf = 0,0,0, 5,0,2, 0,0,0, 0,0,0
+xf = 0,0,0, 5,0,3, 0,0,0, 0,0,0
 
-Qf = 2,2,2, 20,20,20, 5,5,5, 5,5,5
+Qf = 5,5,5, 20,20,20, .5,.5,.5, 5,5,5
 
-Q = .2,.2,.2, .2,.2,.2, .5,.5,.5, .1,.1,.1
+Q = .2,.2,.2, .2,.2,.2, 1,1,1, .01,.01,.01
+#Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0
 
-R = .05, .05, .05, 0.01
+R = .01, .01, .01, 1
diff --git a/bin/qrotortest.cc b/bin/qrotortest.cc
index a0292d8b..2a9f6fcb 100644
--- a/bin/qrotortest.cc
+++ b/bin/qrotortest.cc
@@ -94,6 +94,7 @@ void solver_process(Viewer* viewer)
   if (viewer)
     viewer->Add(view);  
 
+
   struct timeval timer;
   //  ddp.debug = false; // turn off debug for speed
 
diff --git a/lib/algos/ddp.h b/lib/algos/ddp.h
index 14174716..81566623 100644
--- a/lib/algos/ddp.h
+++ b/lib/algos/ddp.h
@@ -103,10 +103,16 @@ namespace gcop {
     double V;
     Vector2d dV;
 
-    double s1;   ///< Armijo/Bertsekas step-size control factor s1
-    double s2;   ///< Armijo/Bertsekas step-size control factor s2
-    double b1;   ///< Armijo/Bertsekas step-size control factor b1
-    double b2;   ///< Armijo/Bertsekas step-size control factor b2
+    double sigma; ///< Armijo sigma factor (0.1 by default)
+    double beta;  ///< Armijo beta factor  (0.25 by default)
+
+    double amin;  ///< minimum step-size: end line-search if a<amin (default is 1e-10)
+    double cVmin; ///< cost-change minimum: end line-search if V_a - V < cVmin (default is 1-10)
+    
+    double s1;   ///< Armijo-Goldstein step-size control factor s1
+    double s2;   ///< Armijo-Goldstein step-size control factor s2
+    double b1;   ///< Armijo-Goldstein step-size control factor b1
+    double b2;   ///< Armijo-Goldstein step-size control factor b2
     
     char type;   ///< type of algorithm (choices are PURE, DDP, LQS), LQS is default. In the current implementation second-order expansion of the dynamics is ignored, so DDP and LQS are identical. Both LQS and DDP are implemented using the true nonlinear dynamics integration in the Forward step, while PURE uses the linearized version in order to match exactly the true Newton step. 
     
@@ -187,6 +193,7 @@ namespace gcop {
     N(us.size()), 
     mu(1e-3), mu0(1e-3), dmu0(2), mumax(1e6), a(1), 
     dus(N), kus(N), Kuxs(N), 
+    sigma(0.1), beta(.25), amin(1e-10), cVmin(1e-10),
     s1(0.1), s2(0.5), b1(0.25), b2(2),
     type(LQS)
     {
@@ -375,11 +382,13 @@ namespace gcop {
     typedef Matrix<double, nu, nu> Matrixcd;  
     
     // measured change in V
-    double dVm = 1;
+    double cV = 1;
     
     double a = this->a;
+
+    bool acc = false;
     
-    while (dVm > 0) {
+    while (1) {
 
       Vectornd dx = VectorXd::Zero(this->sys.X.n);
       dx.setZero();//Redundancy
@@ -407,17 +416,6 @@ namespace gcop {
         Rn<nu> &U = (Rn<nu>&)this->sys.U;
         if (U.bnd) {
           U.Bound(un, du, u);
-          /*
-          for (int j = 0; j < u.size(); ++j) 
-            if (un[j] < U.lb[j]) {
-              un[j] = U.lb[j];
-              du[j] = un[j] - u[j];
-            } else
-              if (un[j] > U.ub[j]) {
-                un[j] = U.ub[j];
-                du[j] = un[j] - u[j];
-              }
-          */
         }
         
         const double &t = this->ts[k];
@@ -468,33 +466,59 @@ namespace gcop {
       
       double L = this->cost.L(this->ts[N], xn, un, 0);
       Vm += L;
+      cV = Vm - V;
       
       if (this->debug)
         cout << "[I] Ddp::Forward: measured V=" << Vm << endl;
       
-      dVm = Vm - V;
+      //      cout << "V-Vm=" << V-Vm << " dV[0]=" << dV[0] << " a=" << a << endl;
+     
+      if (a < amin || fabs(cV) < cVmin)
+        break;
+
+      if (V - Vm < -sigma*a*dV[0]) {
+        a *= beta;
+        continue;
+      } else {
+        break;
+      }
+
       
-      if (dVm > 0) {
+      // the rest below is currently not activated
+
+      // if step is still not acceptable
+      if (acc)
+        break;            
+
+      // Armijo-Goldstein
+      if (cV > 0) {
         a *= b1;
         if (a < 1e-12)
           break;
         if (this->debug)
           cout << "[I] Ddp::Forward: step-size reduced a=" << a << endl;
-        
+
+        acc = false;
         continue;
       }
-      
-      double r = dVm/(a*dV[0] + a*a*dV[1]);
+
+      double r = cV/(a*dV[0] + a*a*dV[1]);
+      //      double r = cV/(a*dV[0]);
       if (r < s1)
         a = b1*a;
       else 
         if (r >= s2) 
-          a = b2*a;    
-    
+          a = b2*a;
+        else
+          break;
+
+      if (a < 1e-12)
+        break;
+      
       if (this->debug)
         cout << "[I] Ddp::Forward: step-size a=" << a << endl;    
     }
-    this->J = V + dVm;//Set the optimal cost after one iteration
+    this->J = V + cV;//Set the optimal cost after one iteration
   }
 
   template <typename T, int nx, int nu, int np> 
diff --git a/lib/algos/docp.h b/lib/algos/docp.h
index 04406829..bd4e7949 100644
--- a/lib/algos/docp.h
+++ b/lib/algos/docp.h
@@ -79,6 +79,8 @@ namespace gcop {
      * @param der whether to update derivatives (A and B matrices)
      */
     void Update(bool der = true);
+
+    double ComputeCost();
     
     System<T, nx, nu, np> &sys;    ///< dynamical system 
 
@@ -141,6 +143,21 @@ namespace gcop {
     Docp<T, nx, nu, np>::~Docp()
     {
     }
+
+  
+  template <typename T, int nx, int nu, int np> 
+    double Docp<T, nx, nu, np>::ComputeCost() {
+    double t = this->ts.back();
+    double J = this->cost.L(t, this->xs.back(), this->us.back(), 0);
+    for (int k = us.size() - 1; k >=0; --k) {      
+      t = this->ts[k];
+      double h = this->ts[k+1] - t;
+      double L = this->cost.L(t, this->xs[k], this->us[k], h);
+      J += L;
+    }    
+    return J;
+  }
+
   
   template <typename T, int nx, int nu, int np> 
     void Docp<T, nx, nu, np>::Update(bool der) {
@@ -157,13 +174,13 @@ namespace gcop {
     
     //    cout << "SIZE=" << ((MbsState*)&xav)->r.size() << endl;
     
-    if (nx == Dynamic) {
+    if (der && nx == Dynamic) {
       dx.resize(sys.X.n);
       dfp.resize(sys.X.n);
       dfm.resize(sys.X.n);
     }
     
-    if (nu == Dynamic)
+    if (der && nu == Dynamic)
       du.resize(sys.U.n);    
 
     int N = us.size();
diff --git a/lib/systems/arm.h b/lib/systems/arm.h
index 0034e0c5..ac2ab380 100644
--- a/lib/systems/arm.h
+++ b/lib/systems/arm.h
@@ -32,7 +32,6 @@ namespace gcop {
     double l2;  ///< second link length
     double x1;  ///< relative offset to second motor in x direction
     
-
   };
 }