From 51e4d3d9c9ff6e271035bfe9a45fdf0dbbb67e88 Mon Sep 17 00:00:00 2001 From: Sylvain Calinon Date: Wed, 30 Jul 2014 17:15:45 +0200 Subject: [PATCH] Adding ARE with Schur or Eig --- demo01.m | 8 ++++---- reproduction_LQR_finiteHorizon.m | 6 ++---- reproduction_LQR_infiniteHorizon.m | 9 +++++---- ...aicRiccati.m => solveAlgebraicRiccati_Schur.m | 0 solveAlgebraicRiccati_eig.m | 16 ++++++++++++++++ 5 files changed, 27 insertions(+), 12 deletions(-) rename solveAlgebraicRiccati.m => solveAlgebraicRiccati_Schur.m (100%) create mode 100644 solveAlgebraicRiccati_eig.m diff --git a/demo01.m b/demo01.m index 5aa772d..701ccdf 100644 --- a/demo01.m +++ b/demo01.m @@ -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 diff --git a/reproduction_LQR_finiteHorizon.m b/reproduction_LQR_finiteHorizon.m index a111ef0..4c70ac1 100644 --- a/reproduction_LQR_finiteHorizon.m +++ b/reproduction_LQR_finiteHorizon.m @@ -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; diff --git a/reproduction_LQR_infiniteHorizon.m b/reproduction_LQR_infiniteHorizon.m index 0d301e6..c012c4d 100644 --- a/reproduction_LQR_infiniteHorizon.m +++ b/reproduction_LQR_infiniteHorizon.m @@ -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; diff --git a/solveAlgebraicRiccati.m b/solveAlgebraicRiccati_Schur.m similarity index 100% rename from solveAlgebraicRiccati.m rename to solveAlgebraicRiccati_Schur.m diff --git a/solveAlgebraicRiccati_eig.m b/solveAlgebraicRiccati_eig.m new file mode 100644 index 0000000..b3bbbeb --- /dev/null +++ b/solveAlgebraicRiccati_eig.m @@ -0,0 +1,16 @@ +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 -- 2.21.0