Commit 878c14e4 authored by Sylvain Calinon's avatar Sylvain Calinon

LQR tests and notational changes

parent 0f1c56b4
......@@ -63,8 +63,8 @@ dtar = gradient(tar,1,2)/model.dt;
%Backward integration of the Riccati equation and additional equation
for t=nbData-1:-1:1
S(:,:,t) = S(:,:,t+1) + model.dt * (A'*S(:,:,t+1) + S(:,:,t+1)*A - S(:,:,t+1) * B * (R\B') * S(:,:,t+1) + Q(:,:,t+1));
d(:,t) = d(:,t+1) - model.dt * (S(:,:,t+1)*B*(R\B') - A') * d(:,t+1) + model.dt * S(:,:,t+1) * dtar(:,t+1) ...
- model.dt * (S(:,:,t+1) * A * tar(:,t+1)); %Optional feedforward term computation
%Optional feedforward term computation
d(:,t) = d(:,t+1) + model.dt * ((A'-S(:,:,t+1)*B*(R\B'))*d(:,t+1) + S(:,:,t+1)*dtar(:,t+1) - S(:,:,t+1)*A*tar(:,t+1));
end
%Computation of the feedback term L in u=-LX+M
for t=1:nbData
......
......@@ -65,7 +65,7 @@ for t=1:nbData
M = R\B'*d;
%Compute acceleration (with only feedback terms)
%ddx = -L * [x-r.currTar(:,t); dx] + M;
%ddx = -L * [x-r.currTar(:,t); dx];
%Compute acceleration (with feedback and feedforward terms)
ddx = -L * [x-r.currTar(:,t); dx] + M;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment