typo fixed

matlab/iLQR_manipulator_recursive_LS.m

@@ -42,8 +42,8 @@ idx = (tl - 1) * param.nbVarX + [1:param.nbVarX]';
 %Transfer matrices (for linear system as single integrator)
 A = eye(param.nbVarX);
 B = eye(param.nbVarX, param.nbVarU) * param.dt;
-A_ = blkdiag(A, 1); %Augmented A
-B_ = [B; zeros(1,param.nbVarU)]; %Augmented B
+Aa = blkdiag(A, 1); %Augmented A
+Ba = [B; zeros(1,param.nbVarU)]; %Augmented B
 
 Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); tril(kron(ones(param.nbData-1), eye(param.nbVarX)*param.dt))];
 Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
@@ -64,30 +64,29 @@ 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
 	
+	%Auxiliary variables
 	Quu = Su' * J' * Q * J * Su + R;
     Qux =  Su' * J' * Q * J * Sx;
     qu = Su' * J' * Q * f(:) + R * uref(:);
     F = Quu \ [Qux, qu];
     
-    %Backward pass
+    %Forward pass
     Ka(:,:,1) = F(1:param.nbVarU,:);
-    k(:,1) = -Ka(:,end,1);
-    K(:,:,1) = -Ka(:,1:end-1,1);
     P = eye(param.nbVarX+1);
     for t=2:param.nbData-1
 	    id = (t-1)*param.nbVarU + [1:param.nbVarU];
-	    P = P / (A_ - B_ * Ka(:,:,t-1));
+	    P = P / (Aa - Ba * Ka(:,:,t-1));
 	    Ka(:,:,t) = F(id,:) * P; %Feedback gain on augmented state
-        k(:,t) = - Ka(:,end,t);
-        K(:,:,t) = -Ka(:,1:end-1,t);
     end
 
-    %Forward pass, including step size estimate (backtracking line search method)
+    %Step size estimate (backtracking line search method)
     alpha = 1;
     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       
+        for t=1:param.nbData-1
+        	k(:,t) = -Ka(:,end,t);
+        	K(:,:,t) = -Ka(:,1:end-1,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
@@ -102,7 +101,6 @@ for n=1:param.nbMaxIter
     end
     uref = uref + du * alpha; %Update control ref
     xref = reshape(Su0 * uref(:) + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
-    
 	
 	if norm(du(:) * alpha) < 1E-3
 		break; %Stop iLQR when solution is reached