Addition of Kp and Kv from L in demos (for analysis)

demo_TPGMR_LQR01.m

@@ -158,13 +158,15 @@ for n=1:nbRepros
 end
 xlabel('t'); ylabel('|Kp|');
 
-%Plot accelerations due to feedback and feedforward terms
-figure; hold on;
-n=1; k=1;
-plot(r(n).FB(k,:),'r-','linewidth',2);
-plot(r(n).FF(k,:),'b-','linewidth',2);
-legend('ddx feedback','ddx feedforward');
-xlabel('t'); ylabel(['ddx_' num2str(k)]);
+
+% %Plot accelerations due to feedback and feedforward terms
+% figure; hold on;
+% n=1; k=1;
+% plot(r(n).FB(k,:),'r-','linewidth',2);
+% plot(r(n).FF(k,:),'b-','linewidth',2);
+% legend('ddx feedback','ddx feedforward');
+% xlabel('t'); ylabel(['ddx_' num2str(k)]);
+
 
 %pause;
 %close all;

demo_TPGMR_LQR02.m

@@ -158,13 +158,15 @@ for n=1:nbRepros
 end
 xlabel('t'); ylabel('|Kp|');
 
-%Plot accelerations due to feedback and feedforward terms
-figure; hold on;
-n=1; k=1;
-plot(r(n).FB(k,:),'r-','linewidth',2);
-plot(r(n).FF(k,:),'b-','linewidth',2);
-legend('ddx feedback','ddx feedforward');
-xlabel('t'); ylabel(['ddx_' num2str(k)]);
+
+% %Plot accelerations due to feedback and feedforward terms
+% figure; hold on;
+% n=1; k=1;
+% plot(r(n).FB(k,:),'r-','linewidth',2);
+% plot(r(n).FF(k,:),'b-','linewidth',2);
+% legend('ddx feedback','ddx feedforward');
+% xlabel('t'); ylabel(['ddx_' num2str(k)]);
+
 
 %pause;
 %close all;

m_fcts/reproduction_LQR_finiteHorizon.m

@@ -80,17 +80,21 @@ for t=1:nbData
 	
 	%Compute acceleration (with both feedback and feedforward terms)
 	ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
-	r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
-	r.FF(:,t) = M(:,t);
 	
 	%Update velocity and position
 	dx = dx + ddx * model.dt;
 	x = x + dx * model.dt;
-	%Log data
+
+	%Log data (with additional variables collected for analysis purpose)
 	r.Data(:,t) = [DataIn(:,t); x];
 	r.ddxNorm(t) = norm(ddx);
+	%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
+	%r.FF(:,t) = M(:,t);
+	%r.Kp(:,:,t) = L(:,1:nbVarOut,t);
+	%r.Kv(:,:,t) = L(:,nbVarOut+1:end,t);
 	r.kpDet(t) = det(L(:,1:nbVarOut,t));
-	r.kvDet(t) = det(L(:,nbVarOut+1:end,t));	
+	r.kvDet(t) = det(L(:,nbVarOut+1:end,t));
+	%Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V'
 end
 
 

m_fcts/reproduction_LQR_infiniteHorizon.m

@@ -67,15 +67,18 @@ for t=1:nbData
 	
 	%Compute acceleration (with feedback and feedforward terms)
 	ddx =  -L * [x-r.currTar(:,t); dx] + M; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
-	r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
-	r.FF(:,t) = M;
 	
 	%Update velocity and position
 	dx = dx + ddx * model.dt;
 	x = x + dx * model.dt;
-	%Log data
+	
+	%Log data (with additional variables collected for analysis purpose)
 	r.Data(:,t) = [DataIn(:,t); x];
 	r.ddxNorm(t) = norm(ddx);
+	%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
+	%r.FF(:,t) = M;
+	%r.Kp(:,:,t) = L(:,1:nbVarOut);
+	%r.Kv(:,:,t) = L(:,nbVarOut+1:end);
 	r.kpDet(t) = det(L(:,1:nbVarOut));
 	r.kvDet(t) = det(L(:,nbVarOut+1:end));
 	%Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V'