From d7c9cf2da67101fdd9d7d99803f8a565744625c5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?J=C3=A9r=C3=A9my=20MACEIRAS?= <jeremy.maceiras@idiap.ch>
Date: Fri, 13 May 2022 12:21:01 +0200
Subject: [PATCH] [matlab] Fixed a typo
---
matlab/LQR_infHor.m | 27 ++++++++++-----------------
1 file changed, 10 insertions(+), 17 deletions(-)
diff --git a/matlab/LQR_infHor.m b/matlab/LQR_infHor.m
index 4468647..baf67c5 100644
--- a/matlab/LQR_infHor.m
+++ b/matlab/LQR_infHor.m
@@ -25,20 +25,20 @@ function LQR_infHor
param.nbData = 200; %Number of datapoints
param.nbRepros = 4; %Number of reproductions
-param.param.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+param.nbVarPos = 2; %Dimension of position data (here: x1,x2)
param.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
-param.nbVar = param.param.nbVarPos * param.nbDeriv; %Dimension of state vector in the tangent space
+param.nbVar = param.nbVarPos * param.nbDeriv; %Dimension of state vector in the tangent space
param.dt = 1E-2; %Time step duration
param.rfactor = 4E-2; %Control cost in LQR
%Control cost matrix
-R = eye(param.param.nbVarPos) * param.rfactor;
+R = eye(param.nbVarPos) * param.rfactor;
%Target and desired covariance
-param.Mu = [randn(param.param.nbVarPos,1); zeros(param.param.nbVarPos*(param.nbDeriv-1),1)];
+param.Mu = [randn(param.nbVarPos,1); zeros(param.nbVarPos*(param.nbDeriv-1),1)];
-[Ar,~] = qr(randn(param.param.nbVarPos));
-xCov = Ar * diag(rand(param.param.nbVarPos,1)) * Ar' * 1E-1
+[Ar,~] = qr(randn(param.nbVarPos));
+xCov = Ar * diag(rand(param.nbVarPos,1)) * Ar' * 1E-1
%% Discrete dynamical System settings
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -50,25 +50,18 @@ B1d = zeros(param.nbDeriv,1);
for i=1:param.nbDeriv
B1d(param.nbDeriv-i+1) = param.dt^i * 1/factorial(i); %Discrete 1D
end
-A = kron(A1d, eye(param.param.nbVarPos)); %Discrete nD
-B = kron(B1d, eye(param.param.nbVarPos)); %Discrete nD
+A = kron(A1d, eye(param.nbVarPos)); %Discrete nD
+B = kron(B1d, eye(param.nbVarPos)); %Discrete nD
%% discrete LQR with infinite horizon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-Q = blkdiag(inv(xCov), zeros(param.param.nbVarPos*(param.nbDeriv-1))); %Precision matrix
+Q = blkdiag(inv(xCov), zeros(param.nbVarPos*(param.nbDeriv-1))); %Precision matrix
P = solveAlgebraicRiccati_eig_discrete(A, B*(R\B'), (Q+Q')/2);
L = (B' * P * B + R) \ B' * P * A; %Feedback gain (discrete version)
-%Test ratio between kp and kv
-if param.nbDeriv>1
- kp = eigs(L(:,1:param.param.nbVarPos));
- kv = eigs(L(:,param.param.nbVarPos+1:end));
- ratio = kv ./ (2 * kp.^.5)
-end
-
for n=1:param.nbRepros
- x = [ones(param.param.nbVarPos,1)+randn(param.param.nbVarPos,1)*5E-1; zeros(param.param.nbVarPos*(param.nbDeriv-1),1)];
+ x = [ones(param.nbVarPos,1)+randn(param.nbVarPos,1)*5E-1; zeros(param.nbVarPos*(param.nbDeriv-1),1)];
for t=1:param.nbData
r(n).Data(:,t) = x;
u = L * (param.Mu - x); %Compute acceleration (with only feedback terms)
--
GitLab