diff --git a/matlab/iLQR_manipulator3D.m b/matlab/iLQR_manipulator3D.m
index 7a0e1d1f6247a0080a5c3d437e95c4ca31817f6d..ff616d55b2a1b7ff9f84dee093640be3a987cf02 100644
--- a/matlab/iLQR_manipulator3D.m
+++ b/matlab/iLQR_manipulator3D.m
@@ -35,7 +35,7 @@ param.dh.r = [0, 0, 0, 0.0825, -0.0825, 0, 0.088, 0]; %Length of the common norm
 %Weight matrices for iLQR cost
 %Q = speye((param.nbVarF-1) * param.nbPoints) * param.q; %Precision matrix
 %Q = kron(eye(param.nbPoints), diag([0, 0, 0, 1E0, 1E0, 1E0])); %Precision matrix (by removing position constraint)
-Q = kron(eye(param.nbPoints), diag([1, 1, 1, 1, 1, 0])); %Precision matrix (by removing orientation constraint on 3rd axis)
+Qr = kron(eye(param.nbPoints), diag([1, 1, 1, 1, 1, 0])); %Precision matrix in relative coordinate frame (tool frame) (by removing orientation constraint on 3rd axis)
 
 R = speye(param.nbVarU * (param.nbData-1)) * param.r; %Control weight matrix (at trajectory level)
 
@@ -83,6 +83,13 @@ Su = Su0(idx,:);
 for n=1:param.nbIter	
 	x = reshape(Su0 * u + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
 	[f, J] = f_reach(x(:,tl), param);
+	Ra = [];
+	for m=1:param.nbPoints
+		Rtmp = q2R(param.Mu(4:end,m)); %Orientation matrix for target
+		Ra = blkdiag(Ra, blkdiag(eye(3),Rtmp)); %Transformation matrix with both translation and rotation 
+	end
+	Q = Ra * Qr * Ra'; %Precision matrix in absolute coordinate frame (base frame)
+
 	du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * param.r); %Gauss-Newton update
 	
 	%Estimate step size with backtracking line search method