Commit 51e4d3d9 authored by Sylvain Calinon's avatar Sylvain Calinon

Adding ARE with Schur or Eig

parent 43f2e613
......@@ -86,8 +86,8 @@ for n=1:nbSamples
a(n) = estimateAttractorPath(DataIn, model, s(n));
%Reproduction with one of the selected approach
r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
%r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
%r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); %This function requires to define model.kP and model.kV (see lines 38-39)
end
......@@ -107,8 +107,8 @@ for n=1:nbRepros
anew(n) = estimateAttractorPath(DataIn, model, rTmp);
%Reproduction with one of the selected approach
rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
%rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
%rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
%rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); %The fct requires to define model.kP and model.kV (see lines 38-39)
end
......
......@@ -52,10 +52,8 @@ P(:,:,nbData) = Pfinal;
%Variables for feedforward term computation (optional for movements with low dynamics)
tar = [r.currTar; zeros(nbVarOut,nbData)];
dtar = gradient(tar,1,2)/model.dt;
%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
%dtar = gradient(tar,1,2)/model.dt;
%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
%dtar = diff([tar tar(:,1)],1,2)/model.dt;
......@@ -82,8 +80,8 @@ for t=1:nbData
%Compute acceleration (with both feedback and feedforward terms)
ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t);
%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M(:,t);
r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
r.FF(:,t) = M(:,t);
%Update velocity and position
dx = dx + ddx * model.dt;
......
......@@ -46,10 +46,11 @@ for t=1:nbData
%care() is a function from the Matlab Control Toolbox to solve the algebraic Riccati equation,
%also available with the control toolbox of GNU Octave
P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
%P = care(A, B, (Q+Q')/2, R); %(Q+Q')/2 is used instead of Q to avoid warnings for the symmetry of Q
%Alternatively the function below can be used for an implementation based on Schur decomposition.
%P = solveAlgebraicRiccati(A, B/R*B', (Q+Q')/2);
%P = solveAlgebraicRiccati_Schur(A, B/R*B', (Q+Q')/2);
P = solveAlgebraicRiccati_eig(A, B/R*B', (Q+Q')/2);
%Variable for feedforward term computation (optional for movements with low dynamics)
d = (P*B*(R\B')-A') \ (P*dtar(:,t) - P*A*tar(:,t));
......@@ -64,8 +65,8 @@ for t=1:nbData
%Compute acceleration (with feedback and feedforward terms)
ddx = -L * [x-r.currTar(:,t); dx] + M;
%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
%r.FF(:,t) = M;
r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
r.FF(:,t) = M;
%Update velocity and position
dx = dx + ddx * model.dt;
......
function X = solveAlgebraicRiccati_eig(A, G, Q)
%Solves the algebraic Riccati equation of the form A'X+XA'-XGX+Q=0, where X is symmetric wiht eigendecomposition.
n = size(A,1);
Z = [A -G; -Q -A'];
[V,D] = eig(Z);
U = [];
for j=1:2*n
if real(D(j,j)) < 0
U = [U V(:,j)];
end
end
X = U(n+1:end,1:n) / U(1:n,1:n);
X = real(X);
\ No newline at end of file
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