Skip to content
Snippets Groups Projects
Commit d3e80791 authored by Sylvain CALINON's avatar Sylvain CALINON
Browse files

typo fixed

parent 2032a84e
Branches
No related tags found
No related merge requests found
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment