diff --git a/matlab/iLQR_manipulator_recursive_LS.m b/matlab/iLQR_manipulator_recursive_LS.m
index 34cd670228283c3f39a20f88dac51b827cfbaa3a..8f540bbe854b2fb78ed9589aa06f2e38c72b8150 100644
--- a/matlab/iLQR_manipulator_recursive_LS.m
+++ b/matlab/iLQR_manipulator_recursive_LS.m
@@ -56,9 +56,6 @@ utmp = zeros(param.nbVarU, param.nbData-1);
 xtmp = zeros(param.nbVarX, param.nbData);
 k = zeros(param.nbVarU, param.nbData-1);
 K = zeros(param.nbVarU, param.nbVarX, param.nbData-1);
-Luu = repmat(eye(param.nbVarU) * param.r, [1,1,param.nbData]);
-Fx = repmat(A, [1,1,param.nbData]);
-Fu = repmat(B, [1,1,param.nbData]);
 
 x0 = [3*pi/4; -pi/2; -pi/4]; %Initial robot pose
 uref = zeros(param.nbVarU, param.nbData-1); %Initial commands
@@ -67,10 +64,10 @@ xref = reshape(Su0 * uref(:) + Sx0 * x0, param.nbVarX, param.nbData); %Initial s
 for n=1:param.nbMaxIter
 	[f, J] = f_reach(xref(:,tl), param); %Residuals and Jacobians
 	
-	Quu_inv = inv(Su' * J' * Q * J * Su + R);
+	Quu = Su' * J' * Q * J * Su + R;
     Qux =  Su' * J' * Q * J * Sx;
     qu = Su' * J' * Q * f(:) + R * uref(:);
-    F = Quu_inv * [Qux, qu];
+    F = Quu \ [Qux, qu];
     
     %Backward pass
     Ka(:,:,1) = F(1:param.nbVarU,:);
@@ -87,16 +84,16 @@ for n=1:param.nbMaxIter
 
     %Forward pass, including step size estimate (backtracking line search method)
     alpha = 1;
-    cost0 = f(:)' * Q * f(:) + norm(uref(:))^2 * param.r; %u' * R * u
+    cost0 = norm(f(:))^2 * param.q + norm(uref(:))^2 * param.r; %u' * R * u
     while 1
         xtmp(:,1) = x0;
         for t=1:param.nbData-1       
-            du(:,t) = alpha * k(:,t) + K(:,:,t) * ( xtmp(:,t) - xref(:,t) );
-	        utmp(:,t) = uref(:,t) + du(:,t);
+            du(:,t) = k(:,t) + K(:,:,t) * (xtmp(:,t) - xref(:,t));
+	        utmp(:,t) = uref(:,t) + du(:,t) * alpha;
 	        xtmp(:,t+1) = A * xtmp(:,t) + B * utmp(:,t); %System evolution
         end
         ftmp = f_reach(xtmp(:,tl), param); %Residuals 
-        cost = ftmp(:)' * Q * ftmp(:) + norm(utmp(:))^2 * param.r; %Cost
+        cost = norm(ftmp(:))^2 * param.q + norm(utmp(:))^2 * param.r; %Cost
         
         if cost < cost0 || alpha < 1E-3
 	        break;