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

typo fixed

parent 2032a84e
Branches
Tags
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