diff --git a/EM_tensorGMM.m b/EM_tensorGMM.m
index b4232e30fad02cc71cae56e45c811fba2411c6e9..4f60aae8c1857231d0b00b3864dded086fedb90b 100644
--- a/EM_tensorGMM.m
+++ b/EM_tensorGMM.m
@@ -1,3 +1,85 @@
+function model = EM_tensorGMM(Data, model)
+% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. 
+% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to 
+% external parameters represented in the form of candidate coordinate systems.   
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+%diagRegularizationFactor = 1E-2;
+diagRegularizationFactor = 1E-4;
+
+for nbIter=1:nbMaxSteps
+	fprintf('.');
+	%E-step
+	[L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below and Eq. (2.0.5) in doc/TechnicalReport.pdf
+	GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+	%M-step
+	for i=1:model.nbStates 
+		%Update Priors
+		model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf
+		for m=1:model.nbFrames
+			%Matricization/flattening of tensor
+			DataMat(:,:) = Data(:,m,:);
+			%Update Mu 
+			model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf
+			%Update Sigma (regularization term is optional) 
+			DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
+			model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; %See Eq. (2.0.8) and (2.1.2) in doc/TechnicalReport.pdf
+		end
+	end
+	%Compute average log-likelihood 
+	LL(nbIter) = sum(log(sum(L,1))) / size(L,2); %See Eq. (2.0.4) in doc/TechnicalReport.pdf
+	%Stop the algorithm if EM converged (small change of LL)
+	if nbIter>nbMinSteps
+		if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+			disp(['EM converged after ' num2str(nbIter) ' iterations.']); 
+			return;
+		end
+	end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']); 
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [L, GAMMA, GAMMA0] = computeGamma(Data, model)
+	%See Eq. (2.0.5) in doc/TechnicalReport.pdf
+	nbData = size(Data, 3);
+	L = ones(model.nbStates, nbData);
+	GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+	for m=1:model.nbFrames
+		DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
+		for i=1:model.nbStates
+			GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
+			L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))'; 
+		end
+	end
+	%Normalization
+	GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1);
+end
+
+
+
+
 function model = EM_tensorGMM(Data, model)
 % Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. 
 % The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to 
diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m
index f136b698065770746d2a77d74e621453eff4ac57..6630adb2996318b6efc7a683ed6107e771c4dcc9 100644
--- a/demo_DSGMR01.m
+++ b/demo_DSGMR01.m
@@ -1,183 +1,183 @@
-function demo_DSGMR01
-% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
-% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
-% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
-% relevance of the frame through the task. This information is exploited to generate a new attractor path 
-% corresponding to new situations (new positions and orientation of the frames).
-%
-% This demo presents the results for a dynamical system with constant gains.
-%  
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbStates = 3; %Number of Gaussians in the GMM
-model.nbFrames = 2; %Number of candidate frames of reference
-model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
-model.dt = 0.01; %Time step 
-model.kP = 100; %Stiffness gain 
-model.kV = (2*model.kP)^.5; %Damping gain 
-nbRepros = 8; %Number of reproductions with new situations randomly generated
-
-%% Load 3rd order tensor data
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Load 3rd order tensor data...');
-% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
-% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
-% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
-% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
-% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
-% multiplied by the number of demonstrations (5).
-load('data/DataLQR01.mat');
-
-
-%% Transformation of 'Data' to learn the path of the spring-damper system
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbD = s(1).nbData;
-nbVarOut = model.nbVar - 1;
-%Create transformation matrix to compute [X; DX; DDX]
-D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
-D(end,end) = 0;
-%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
-K1d = [1, model.kV/model.kP, 1/model.kP];
-K = kron(K1d,eye(nbVarOut));
-%Create 3rd order tensor data with XHAT instead of X
-for n=1:nbSamples
-	DataTmp = s(n).Data0(2:end,:);
-	DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
-	for m=1:model.nbFrames
-		Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD)); 
-	end
-end
-
-
-%% Tensor GMM learning
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of tensor GMM with EM:');
-model = init_tensorGMM_timeBased(Data, model); %Initialization 
-model = EM_tensorGMM(Data, model);
-
-
-%% Reproduction with DS-GMR for the task parameters used to train the model
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with DS-GMR...');
-DataIn = [1:s(1).nbData] * model.dt;
-for n=1:nbSamples
-  %Retrieval of attractor path through task-parameterized GMR
-  a(n) = estimateAttractorPath(DataIn, model, s(n));
-  r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); 
-end
-
-
-%% Reproduction with DS-GMR for new task parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('New reproductions with DS-GMR...');
-for n=1:nbRepros 
-  for m=1:model.nbFrames
-    %Random generation of new task parameters 
-    id=ceil(rand(2,1)*nbSamples);
-    w=rand(2); w=w/sum(w);
-    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
-    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
-  end
-  %Retrieval of attractor path through task-parameterized GMR
-  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
-  rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); 
-end
-
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,1300,500]);
-xx = round(linspace(1,64,nbSamples));
-clrmap = colormap('jet');
-clrmap = min(clrmap(xx,:),.95);
-limAxes = [-1.2 0.8 -1.1 0.9];
-colPegs = [[.9,.5,.9];[.5,.9,.5]];
-
-%DEMOS
-subplot(1,3,1); hold on; box on; title('Demonstrations');
-for n=1:nbSamples
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot trajectories
-  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
-  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%REPROS
-subplot(1,3,2); hold on; box on; title('Reproductions with DS-GMR');
-for n=1:nbSamples
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot Gaussians
-  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
-end
-for n=1:nbSamples
-  %Plot trajectories
-  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
-  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%NEW REPROS
-subplot(1,3,3); hold on; box on; title('New reproductions with DS-GMR');
-for n=1:nbRepros
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot Gaussians
-  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
-end
-for n=1:nbRepros
-  %Plot trajectories
-  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
-  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%print('-dpng','outTest1.png');
-
-%Plot additional information 
-figure; 
-%Plot norm of control commands
-subplot(1,2,1); hold on;
-for n=1:nbRepros
-  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('|ddx|');
-%Plot strength of the stiffness term
-subplot(1,2,2); hold on;
-for n=1:nbRepros
-  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('|Kp|');
-
-
-%pause;
-%close all;
-
-
+function demo_DSGMR01
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
+% relevance of the frame through the task. This information is exploited to generate a new attractor path 
+% corresponding to new situations (new positions and orientation of the frames).
+%
+% This demo presents the results for a dynamical system with constant gains.
+%  
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step 
+model.kP = 100; %Stiffness gain 
+model.kV = (2*model.kP)^.5; %Damping gain 
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Transformation of 'Data' to learn the path of the spring-damper system
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbD = s(1).nbData;
+nbVarOut = model.nbVar - 1;
+%Create transformation matrix to compute [X; DX; DDX]
+D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
+D(end,end) = 0;
+%Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
+K1d = [1, model.kV/model.kP, 1/model.kP];
+K = kron(K1d,eye(nbVarOut));
+%Create 3rd order tensor data with XHAT instead of X
+for n=1:nbSamples
+	DataTmp = s(n).Data0(2:end,:);
+	DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
+	for m=1:model.nbFrames
+		Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD)); 
+	end
+end
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization 
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with DS-GMR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with DS-GMR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+  %Retrieval of attractor path through task-parameterized GMR
+  a(n) = estimateAttractorPath(DataIn, model, s(n));
+  r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); 
+end
+
+
+%% Reproduction with DS-GMR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with DS-GMR...');
+for n=1:nbRepros 
+  for m=1:model.nbFrames
+    %Random generation of new task parameters 
+    id=ceil(rand(2,1)*nbSamples);
+    w=rand(2); w=w/sum(w);
+    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+  end
+  %Retrieval of attractor path through task-parameterized GMR
+  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+  rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); 
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot trajectories
+  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with DS-GMR');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+  %Plot trajectories
+  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with DS-GMR');
+for n=1:nbRepros
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+  %Plot trajectories
+  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information 
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|Kp|');
+
+
+%pause;
+%close all;
+
+
diff --git a/demo_TPGMR_LQR01.m b/demo_TPGMR_LQR01.m
index b1b5c3238db936ff2cbe9e28bdd687e2b8edb7a2..78d1e8aa3d6a613d2754c5d59ef8ae53ad08946a 100644
--- a/demo_TPGMR_LQR01.m
+++ b/demo_TPGMR_LQR01.m
@@ -1,173 +1,346 @@
-function demo_TPGMR_LQR01
-% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
-% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
-% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
-% relevance of the frame through the task. This information is exploited to generate a new attractor path 
-% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
-% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
-% the spring-damper systems, resulting in a minimal intervention control strategy.
-%
-% This demo presents the results for a finite horizon LQR.
-%  
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbStates = 3; %Number of Gaussians in the GMM
-model.nbFrames = 2; %Number of candidate frames of reference
-model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
-model.dt = 0.01; %Time step 
-nbRepros = 8; %Number of reproductions with new situations randomly generated
-rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
-
-
-%% Load 3rd order tensor data
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Load 3rd order tensor data...');
-% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
-% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
-% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
-% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
-% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
-% multiplied by the number of demonstrations (5).
-load('data/DataLQR01.mat');
-
-
-%% Tensor GMM learning
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of tensor GMM with EM:');
-model = init_tensorGMM_timeBased(Data, model); %Initialization 
-model = EM_tensorGMM(Data, model);
-
-
-%% Reproduction with LQR for the task parameters used to train the model
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with LQR...');
-DataIn = [1:s(1).nbData] * model.dt;
-for n=1:nbSamples
-  %Retrieval of attractor path through task-parameterized GMR
-  a(n) = estimateAttractorPath(DataIn, model, s(n));
-  r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
-end
-
-
-%% Reproduction with LQR for new task parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('New reproductions with LQR...');
-for n=1:nbRepros 
-  for m=1:model.nbFrames
-    %Random generation of new task parameters 
-    id=ceil(rand(2,1)*nbSamples);
-    w=rand(2); w=w/sum(w);
-    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
-    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
-  end
-  %Retrieval of attractor path through task-parameterized GMR
-  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
-  rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
-end
-
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,1300,500]);
-xx = round(linspace(1,64,nbSamples));
-clrmap = colormap('jet');
-clrmap = min(clrmap(xx,:),.95);
-limAxes = [-1.2 0.8 -1.1 0.9];
-colPegs = [[.9,.5,.9];[.5,.9,.5]];
-
-%DEMOS
-subplot(1,3,1); hold on; box on; title('Demonstrations');
-for n=1:nbSamples
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot trajectories
-  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
-  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%REPROS
-subplot(1,3,2); hold on; box on; title('Reproductions with finite horizon LQR');
-for n=1:nbSamples
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot Gaussians
-  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
-end
-for n=1:nbSamples
-  %Plot trajectories
-  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
-  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%NEW REPROS
-subplot(1,3,3); hold on; box on; title('New reproductions with finite horizon LQR');
-for n=1:nbRepros
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot Gaussians
-  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
-end
-for n=1:nbRepros
-  %Plot trajectories
-  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
-  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%print('-dpng','outTest1.png');
-
-%Plot additional information 
-figure; 
-%Plot norm of control commands
-subplot(1,2,1); hold on;
-for n=1:nbRepros
-  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('|ddx|');
-%Plot strength of the stiffness term
-subplot(1,2,2); hold on;
-for n=1:nbRepros
-  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
-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)]);
-
-%print('-dpng','outTest2.png');
-%pause;
-%close all;
-
-
+function demo_TPGMR_LQR01
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
+% relevance of the frame through the task. This information is exploited to generate a new attractor path 
+% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
+% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
+% the spring-damper systems, resulting in a minimal intervention control strategy.
+%
+% This demo presents the results for a finite horizon LQR.
+%  
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step 
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization 
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+	%Retrieval of attractor path through task-parameterized GMR
+	a(n) = estimateAttractorPath(DataIn, model, s(n));
+	r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros 
+	for m=1:model.nbFrames
+		%Random generation of new task parameters 
+		id=ceil(rand(2,1)*nbSamples);
+		w=rand(2); w=w/sum(w);
+		rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+		rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+	end
+	%Retrieval of attractor path through task-parameterized GMR
+	anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+	rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with finite horizon LQR');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot Gaussians
+	plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+	%Plot trajectories
+	plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with finite horizon LQR');
+for n=1:nbRepros
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot Gaussians
+	plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+	%Plot trajectories
+	plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+	plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information 
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+	plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+	plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+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)]);
+
+%print('-dpng','outTest2.png');
+%pause;
+%close all;
+
+
+function demo_TPGMR_LQR01
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
+% relevance of the frame through the task. This information is exploited to generate a new attractor path 
+% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
+% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
+% the spring-damper systems, resulting in a minimal intervention control strategy.
+%
+% This demo presents the results for a finite horizon LQR.
+%  
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step 
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization 
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+  %Retrieval of attractor path through task-parameterized GMR
+  a(n) = estimateAttractorPath(DataIn, model, s(n));
+  r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros 
+  for m=1:model.nbFrames
+    %Random generation of new task parameters 
+    id=ceil(rand(2,1)*nbSamples);
+    w=rand(2); w=w/sum(w);
+    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+  end
+  %Retrieval of attractor path through task-parameterized GMR
+  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+  rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot trajectories
+  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with finite horizon LQR');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+  %Plot trajectories
+  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with finite horizon LQR');
+for n=1:nbRepros
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+  %Plot trajectories
+  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information 
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+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)]);
+
+%print('-dpng','outTest2.png');
+%pause;
+%close all;
+
+
diff --git a/demo_TPGMR_LQR02.m b/demo_TPGMR_LQR02.m
index ff914e2b7513cbdecb0c17fc03b19ee021291d95..2b77de7a448dee84330d1b290528c25f2c32d1f3 100644
--- a/demo_TPGMR_LQR02.m
+++ b/demo_TPGMR_LQR02.m
@@ -1,173 +1,346 @@
-function demo_TPGMR_LQR02
-% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
-% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
-% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
-% relevance of the frame through the task. This information is exploited to generate a new attractor path 
-% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
-% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
-% the spring-damper systems, resulting in a minimal intervention control strategy.
-%
-% This demo presents the results for an infinite horizon LQR.
-%  
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbStates = 3; %Number of Gaussians in the GMM
-model.nbFrames = 2; %Number of candidate frames of reference
-model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
-model.dt = 0.01; %Time step 
-nbRepros = 8; %Number of reproductions with new situations randomly generated
-rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
-
-
-%% Load 3rd order tensor data
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Load 3rd order tensor data...');
-% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
-% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
-% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
-% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
-% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
-% multiplied by the number of demonstrations (5).
-load('data/DataLQR01.mat');
-
-
-%% Tensor GMM learning
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of tensor GMM with EM:');
-model = init_tensorGMM_timeBased(Data, model); %Initialization 
-model = EM_tensorGMM(Data, model);
-
-
-%% Reproduction with LQR for the task parameters used to train the model
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with LQR...');
-DataIn = [1:s(1).nbData] * model.dt;
-for n=1:nbSamples
-  %Retrieval of attractor path through task-parameterized GMR
-  a(n) = estimateAttractorPath(DataIn, model, s(n));  
-  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
-end
-
-
-%% Reproduction with LQR for new task parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('New reproductions with LQR...');
-for n=1:nbRepros 
-  for m=1:model.nbFrames
-    %Random generation of new task parameters 
-    id=ceil(rand(2,1)*nbSamples);
-    w=rand(2); w=w/sum(w);
-    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
-    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
-  end
-  %Retrieval of attractor path through task-parameterized GMR
-  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
-  rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
-end
-
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,1300,500]);
-xx = round(linspace(1,64,nbSamples));
-clrmap = colormap('jet');
-clrmap = min(clrmap(xx,:),.95);
-limAxes = [-1.2 0.8 -1.1 0.9];
-colPegs = [[.9,.5,.9];[.5,.9,.5]];
-
-%DEMOS
-subplot(1,3,1); hold on; box on; title('Demonstrations');
-for n=1:nbSamples
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot trajectories
-  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
-  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%REPROS
-subplot(1,3,2); hold on; box on; title('Reproductions with infinite horizon LQR');
-for n=1:nbSamples
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot Gaussians
-  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
-end
-for n=1:nbSamples
-  %Plot trajectories
-  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
-  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%NEW REPROS
-subplot(1,3,3); hold on; box on; title('New reproductions with infinite horizon LQR');
-for n=1:nbRepros
-  %Plot frames
-  for m=1:model.nbFrames
-    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
-    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
-  end
-  %Plot Gaussians
-  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
-end
-for n=1:nbRepros
-  %Plot trajectories
-  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
-  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
-end
-axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
-
-%print('-dpng','outTest1.png');
-
-%Plot additional information 
-figure; 
-%Plot norm of control commands
-subplot(1,2,1); hold on;
-for n=1:nbRepros
-  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('|ddx|');
-%Plot strength of the stiffness term
-subplot(1,2,2); hold on;
-for n=1:nbRepros
-  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
-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)]);
-
-%print('-dpng','outTest2.png');
-%pause;
-%close all;
-
-
+function demo_TPGMR_LQR02
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
+% relevance of the frame through the task. This information is exploited to generate a new attractor path 
+% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
+% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
+% the spring-damper systems, resulting in a minimal intervention control strategy.
+%
+% This demo presents the results for an infinite horizon LQR.
+%  
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step 
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization 
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+	%Retrieval of attractor path through task-parameterized GMR
+	a(n) = estimateAttractorPath(DataIn, model, s(n));  
+	r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros 
+	for m=1:model.nbFrames
+		%Random generation of new task parameters 
+		id=ceil(rand(2,1)*nbSamples);
+		w=rand(2); w=w/sum(w);
+		rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+		rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+	end
+	%Retrieval of attractor path through task-parameterized GMR
+	anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+	rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with infinite horizon LQR');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot Gaussians
+	plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+	%Plot trajectories
+	plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with infinite horizon LQR');
+for n=1:nbRepros
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot Gaussians
+	plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+	%Plot trajectories
+	plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+	plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information 
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+	plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+	plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+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)]);
+
+%print('-dpng','outTest2.png');
+%pause;
+%close all;
+
+
+function demo_TPGMR_LQR02
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
+% relevance of the frame through the task. This information is exploited to generate a new attractor path 
+% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
+% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
+% the spring-damper systems, resulting in a minimal intervention control strategy.
+%
+% This demo presents the results for an infinite horizon LQR.
+%  
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step 
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization 
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+  %Retrieval of attractor path through task-parameterized GMR
+  a(n) = estimateAttractorPath(DataIn, model, s(n));  
+  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros 
+  for m=1:model.nbFrames
+    %Random generation of new task parameters 
+    id=ceil(rand(2,1)*nbSamples);
+    w=rand(2); w=w/sum(w);
+    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+  end
+  %Retrieval of attractor path through task-parameterized GMR
+  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+  rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot trajectories
+  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with infinite horizon LQR');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+  %Plot trajectories
+  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with infinite horizon LQR');
+for n=1:nbRepros
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+  %Plot trajectories
+  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','outTest1.png');
+
+%Plot additional information 
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+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)]);
+
+%print('-dpng','outTest2.png');
+%pause;
+%close all;
+
+
diff --git a/demo_testLQR01.m b/demo_testLQR01.m
index 5a9283211b983c44367c3c674bdb02d12520c62e..7e0290ad3e3bf281a3cc0b1f46fb2b7c3e09d262 100644
--- a/demo_testLQR01.m
+++ b/demo_testLQR01.m
@@ -1,66 +1,132 @@
-function demo_testLQR01
-% Test of the linear quadratic regulation
-%
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
-model.dt = 0.01; %Time step 
-nbData = 500; %Number of datapoints
-nbRepros = 3; %Number of reproductions with new situations randomly generated
-rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
-
-%% Reproduction with LQR 
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with LQR...');
-DataIn = [1:nbData] * model.dt;
-a.currTar = ones(1,nbData); 
-for n=1:nbRepros
-  a.currSigma = ones(1,1,nbData) * 10^(2-n);
-  %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
-  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
-end
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,1300,500]);
-hold on; box on;
-%Plot target
-plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
-for n=1:nbRepros
-  %Plot trajectories
-  plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('x_1');
-
-figure; 
-%Plot norm of control commands
-subplot(1,2,1); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('|ddx|');
-%Plot stiffness
-subplot(1,2,2); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('kp');
-
-%pause;
-%close all;
+function demo_testLQR01
+% Test of the linear quadratic regulation
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 500; %Number of datapoints
+nbRepros = 3; %Number of reproductions with new situations randomly generated
+rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+a.currTar = ones(1,nbData); 
+for n=1:nbRepros
+	a.currSigma = ones(1,1,nbData) * 10^(2-n);
+	%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
+	r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on;
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+	%Plot trajectories
+	plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kp');
+
+%pause;
+%close all;
+function demo_testLQR01
+% Test of the linear quadratic regulation
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 500; %Number of datapoints
+nbRepros = 3; %Number of reproductions with new situations randomly generated
+rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+a.currTar = ones(1,nbData); 
+for n=1:nbRepros
+  a.currSigma = ones(1,1,nbData) * 10^(2-n);
+  %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
+  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on;
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+  %Plot trajectories
+  plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kp');
+
+%pause;
+%close all;
diff --git a/demo_testLQR02.m b/demo_testLQR02.m
index 9d4489f7df49d0c213e682819e9855012a6e8aab..f7a765152b4d8df95efa631faa2bf44c6b340d70 100644
--- a/demo_testLQR02.m
+++ b/demo_testLQR02.m
@@ -1,74 +1,148 @@
-function demo_testLQR02
-% Test of the linear quadratic regulation
-%
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
-model.dt = 0.01; %Time step 
-nbData = 1000; %Number of datapoints
-nbRepros = 1; %Number of reproductions with new situations randomly generated
-rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
-
-%% Reproduction with LQR 
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with LQR...');
-DataIn = [1:nbData] * model.dt;
-a.currTar = ones(1,nbData);
-a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u 
-for n=1:nbRepros
-  %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
-  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
-end
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,1300,500]);
-hold on; box on; 
-%Plot target
-plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
-for n=1:nbRepros
-  %Plot trajectories
-  plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('x_1');
-
-figure; 
-%Plot norm of control commands
-subplot(1,3,1); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('|ddx|');
-%Plot stiffness
-subplot(1,3,2); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('kp');
-%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5)
-subplot(1,3,3); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2);
-end
-xlabel('t'); ylabel('kp/kv');
-
-r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071
-
-%pause;
-%close all;
+function demo_testLQR02
+% Test of the linear quadratic regulation
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 1000; %Number of datapoints
+nbRepros = 1; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+a.currTar = ones(1,nbData);
+a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u 
+for n=1:nbRepros
+	%r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
+	r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on; 
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+	%Plot trajectories
+	plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot norm of control commands
+subplot(1,3,1); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(1,3,2); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('kp');
+%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5)
+subplot(1,3,3); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('kp/kv');
+
+r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071
+
+%pause;
+%close all;
+function demo_testLQR02
+% Test of the linear quadratic regulation
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 1000; %Number of datapoints
+nbRepros = 1; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+a.currTar = ones(1,nbData);
+a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u 
+for n=1:nbRepros
+  %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
+  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on; 
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+  %Plot trajectories
+  plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot norm of control commands
+subplot(1,3,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(1,3,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('kp');
+%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5)
+subplot(1,3,3); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('kp/kv');
+
+r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071
+
+%pause;
+%close all;
diff --git a/demo_testLQR03.m b/demo_testLQR03.m
index 97bcbe1e1cf6236d427d46d0ad8604eb8bdd9223..d1c51f729d28cdc38321249de3690c0a6d95f68f 100644
--- a/demo_testLQR03.m
+++ b/demo_testLQR03.m
@@ -1,99 +1,198 @@
-function demo_testLQR03
-% Comaprison of linear quadratic regulators with finite and infinite time horizons
-%
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
-model.dt = 0.01; %Time step 
-nbData = 400; %Number of datapoints
-nbRepros = 2; %Number of reproductions with new situations randomly generated
-rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
-
-%% Reproduction with LQR 
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-disp('Reproductions with LQR...');
-DataIn = [1:nbData] * model.dt;
-%a.currTar = ones(1,nbData); 
-a.currTar = linspace(100,100,nbData); 
-%a.currTar = sin(linspace(0,8*pi,nbData)); 
-
-%a.currSigma = ones(1,1,nbData);
-a.currSigma = ones(1,1,nbData-100) * 100; 
-a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1;
-aFinal.currTar = a.currTar(:,end); 
-aFinal.currSigma = a.currSigma(:,:,end);
-    
-for n=1:nbRepros
-  if n==1
-    r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
-  else
-    %First call to LQR to get an estimate of the final feedback terms 
-    [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
-    %Second call to LQR with finite horizon 
-    r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
-  end
-end
-for n=1:nbRepros
-  %Evaluation of determinant (for analysis purpose)
-  for t=1:nbData
-    r(n).detSigma(t) = det(a.currSigma(:,:,t));
-  end
-end
-
-%% Plots
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[20,50,1300,500]);
-hold on; box on;
-%Plot target
-plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
-for n=1:nbRepros
-  %Plot trajectories
-  plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('x_1');
-
-figure; 
-%Plot variations
-subplot(2,3,[1,4]); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('|\Sigma|');
-%Plot norm of control commands
-subplot(2,3,[2,5]); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('|ddx|');
-%Plot stiffness
-subplot(2,3,3); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('kp');
-%Plot damping
-subplot(2,3,6); hold on;
-for n=1:nbRepros
-  plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
-end
-xlabel('t'); ylabel('kv');
-
-%pause;
-%close all;
-
+function demo_testLQR03
+% Comaprison of linear quadratic regulators with finite and infinite time horizons
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 400; %Number of datapoints
+nbRepros = 2; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+%a.currTar = ones(1,nbData); 
+a.currTar = linspace(100,100,nbData); 
+%a.currTar = sin(linspace(0,8*pi,nbData)); 
+
+%a.currSigma = ones(1,1,nbData);
+a.currSigma = ones(1,1,nbData-100) * 100; 
+a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1;
+aFinal.currTar = a.currTar(:,end); 
+aFinal.currSigma = a.currSigma(:,:,end);
+		
+for n=1:nbRepros
+	if n==1
+		r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+	else
+		%First call to LQR to get an estimate of the final feedback terms 
+		[~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
+		%Second call to LQR with finite horizon 
+		r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
+	end
+end
+for n=1:nbRepros
+	%Evaluation of determinant (for analysis purpose)
+	for t=1:nbData
+		r(n).detSigma(t) = det(a.currSigma(:,:,t));
+	end
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on;
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+	%Plot trajectories
+	plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot variations
+subplot(2,3,[1,4]); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|\Sigma|');
+%Plot norm of control commands
+subplot(2,3,[2,5]); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(2,3,3); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kp');
+%Plot damping
+subplot(2,3,6); hold on;
+for n=1:nbRepros
+	plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kv');
+
+%pause;
+%close all;
+
+function demo_testLQR03
+% Comaprison of linear quadratic regulators with finite and infinite time horizons
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 400; %Number of datapoints
+nbRepros = 2; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+%a.currTar = ones(1,nbData); 
+a.currTar = linspace(100,100,nbData); 
+%a.currTar = sin(linspace(0,8*pi,nbData)); 
+
+%a.currSigma = ones(1,1,nbData);
+a.currSigma = ones(1,1,nbData-100) * 100; 
+a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1;
+aFinal.currTar = a.currTar(:,end); 
+aFinal.currSigma = a.currSigma(:,:,end);
+    
+for n=1:nbRepros
+  if n==1
+    r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+  else
+    %First call to LQR to get an estimate of the final feedback terms 
+    [~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
+    %Second call to LQR with finite horizon 
+    r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
+  end
+end
+for n=1:nbRepros
+  %Evaluation of determinant (for analysis purpose)
+  for t=1:nbData
+    r(n).detSigma(t) = det(a.currSigma(:,:,t));
+  end
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on;
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+  %Plot trajectories
+  plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot variations
+subplot(2,3,[1,4]); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|\Sigma|');
+%Plot norm of control commands
+subplot(2,3,[2,5]); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(2,3,3); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kp');
+%Plot damping
+subplot(2,3,6); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kv');
+
+%pause;
+%close all;
+
diff --git a/estimateAttractorPath.m b/estimateAttractorPath.m
index 62653ded260d212ac31ae2a58b510be2d39a8821..ca5d3a4f85e9da4c2a0269108ac2ab971bd63727 100644
--- a/estimateAttractorPath.m
+++ b/estimateAttractorPath.m
@@ -32,3 +32,37 @@ r.nbStates = model.nbStates;
 
 
 
+function r = estimateAttractorPath(DataIn, model, r)
+% Estimation of an attractor path from a task-parameterized GMM and a set of candidate frames.
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+in = 1:size(DataIn,1);
+out = in(end)+1:model.nbVar;
+
+%% Estimation of the attractor path by Gaussian mixture regression,
+%% by using the GMM resulting from the product of linearly transformed Gaussians
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+[r.Mu, r.Sigma] = productTPGMM(model, r.p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
+r.Priors = model.Priors;  
+r.nbStates = model.nbStates;
+
+[r.currTar, r.currSigma] = GMR(r, DataIn, in, out); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+
+
+
diff --git a/gaussPDF.m b/gaussPDF.m
index f53aee739d83f18606dbbc0cb5c55b6093642ed1..326a26bb37433ee6eccd46adfde5abfedf4fcbc2 100644
--- a/gaussPDF.m
+++ b/gaussPDF.m
@@ -16,3 +16,21 @@ function prob = gaussPDF(Data, Mu, Sigma)
 Data = Data' - repmat(Mu',nbData,1);
 prob = sum((Data/Sigma).*Data, 2);
 prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin));
+function prob = gaussPDF(Data, Mu, Sigma)
+% Likelihood of datapoint(s) to be generated by a Gaussian parameterized by center and covariance.
+%
+% Inputs -----------------------------------------------------------------
+%   o Data:  D x N array representing N datapoints of D dimensions.
+%   o Mu:    D x 1 vector representing the center of the Gaussian.
+%   o Sigma: D x D array representing the covariance matrix of the Gaussian.
+% Output -----------------------------------------------------------------
+%   o prob:  1 x N vector representing the likelihood of the N datapoints.     
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+
+[nbVar,nbData] = size(Data);
+%See Eq. (2.0.3) in doc/TechnicalReport.pdf
+Data = Data' - repmat(Mu',nbData,1);
+prob = sum((Data/Sigma).*Data, 2);
+prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin));
diff --git a/init_tensorGMM_kmeans.m b/init_tensorGMM_kmeans.m
index 3fc3eaa84e3a70705a73bf4bad02795268063a01..a0089e69a260744e1583b9e9928808f44f3aae02 100644
--- a/init_tensorGMM_kmeans.m
+++ b/init_tensorGMM_kmeans.m
@@ -11,18 +11,18 @@ DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization
 [Data_id, Mu] = kmeansClustering(DataAll, model.nbStates);
 
 for i=1:model.nbStates
-  idtmp = find(Data_id==i);
-  model.Priors(i) = length(idtmp);
-  Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor;
+	idtmp = find(Data_id==i);
+	model.Priors(i) = length(idtmp);
+	Sigma(:,:,i) = cov([DataAll(:,idtmp) DataAll(:,idtmp)]') + eye(size(DataAll,1))*diagRegularizationFactor;
 end
 model.Priors = model.Priors / sum(model.Priors);
 
 %Reshape GMM parameters into a tensor
 for m=1:model.nbFrames
-  for i=1:model.nbStates
-    model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i);
-    model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
-  end
+	for i=1:model.nbStates
+		model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i);
+		model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i);
+	end
 end
 
 
diff --git a/init_tensorGMM_timeBased.m b/init_tensorGMM_timeBased.m
index 010c80ba64a3fe4f1e50740ef5eeeb1de1bb3642..fdfe089f8afb5e95fb1f36c6a90b62550db82b96 100644
--- a/init_tensorGMM_timeBased.m
+++ b/init_tensorGMM_timeBased.m
@@ -1,27 +1,27 @@
-function model = init_tensorGMM_timeBased(Data, model)
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-
-diagRegularizationFactor = 1E-4;
-
-DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
-
-TimingSep = linspace(min(DataAll(1,:)), max(DataAll(1,:)), model.nbStates+1);
-Mu = zeros(model.nbFrames*model.nbVar, model.nbStates);
-Sigma = zeros(model.nbFrames*model.nbVar, model.nbFrames*model.nbVar, model.nbStates);
-for i=1:model.nbStates
-  idtmp = find( DataAll(1,:)>=TimingSep(i) & DataAll(1,:)<TimingSep(i+1));
-  Mu(:,i) = mean(DataAll(:,idtmp),2);
-  Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor;
-  model.Priors(i) = length(idtmp);
-end
-model.Priors = model.Priors / sum(model.Priors);
-
-%Reshape GMM parameters into a tensor 
-for m=1:model.nbFrames
-  for i=1:model.nbStates
-    model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); 
-    model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); 
-  end
-end
-
+function model = init_tensorGMM_timeBased(Data, model)
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+
+diagRegularizationFactor = 1E-4;
+
+DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
+
+TimingSep = linspace(min(DataAll(1,:)), max(DataAll(1,:)), model.nbStates+1);
+Mu = zeros(model.nbFrames*model.nbVar, model.nbStates);
+Sigma = zeros(model.nbFrames*model.nbVar, model.nbFrames*model.nbVar, model.nbStates);
+for i=1:model.nbStates
+	idtmp = find( DataAll(1,:)>=TimingSep(i) & DataAll(1,:)<TimingSep(i+1));
+	Mu(:,i) = mean(DataAll(:,idtmp),2);
+	Sigma(:,:,i) = cov(DataAll(:,idtmp)') + eye(size(DataAll,1))*diagRegularizationFactor;
+	model.Priors(i) = length(idtmp);
+end
+model.Priors = model.Priors / sum(model.Priors);
+
+%Reshape GMM parameters into a tensor 
+for m=1:model.nbFrames
+	for i=1:model.nbStates
+		model.Mu(:,m,i) = Mu((m-1)*model.nbVar+1:m*model.nbVar,i); 
+		model.Sigma(:,:,m,i) = Sigma((m-1)*model.nbVar+1:m*model.nbVar,(m-1)*model.nbVar+1:m*model.nbVar,i); 
+	end
+end
+
diff --git a/kmeansClustering.m b/kmeansClustering.m
index c5e5696b9f8287279c9a044b9be5e8b5d75f15c9..894585fb7d6ec702e6117a46c61f31b8e067ed45 100644
--- a/kmeansClustering.m
+++ b/kmeansClustering.m
@@ -17,24 +17,24 @@ Mu = Data(:,idTmp(1:nbStates));
 
 %k-means iterations
 while 1
-  %E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-  for i=1:nbStates
-    %Compute distances
-    distTmp(:,i) = sum((Data-repmat(Mu(:,i),1,nbData)).^2);
-  end
-  [vTmp,idList] = min(distTmp,[],2);
-  cumdist = sum(vTmp);
-  %M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-  for i=1:nbStates
-    %Update the centers
-    Mu(:,i) = mean(Data(:,idList==i),2);
-  end
-  %Stopping criterion %%%%%%%%%%%%%%%%%%%%
-  if abs(cumdist-cumdist_old) < cumdist_threshold
-    break;
-  end
-  cumdist_old = cumdist;
-  nbStep = nbStep+1;
+	%E-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+	for i=1:nbStates
+		%Compute distances
+		distTmp(:,i) = sum((Data-repmat(Mu(:,i),1,nbData)).^2);
+	end
+	[vTmp,idList] = min(distTmp,[],2);
+	cumdist = sum(vTmp);
+	%M-step %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+	for i=1:nbStates
+		%Update the centers
+		Mu(:,i) = mean(Data(:,idList==i),2);
+	end
+	%Stopping criterion %%%%%%%%%%%%%%%%%%%%
+	if abs(cumdist-cumdist_old) < cumdist_threshold
+		break;
+	end
+	cumdist_old = cumdist;
+	nbStep = nbStep+1;
 %   if nbStep>maxIter
 %     disp(['Maximum number of iterations, ' num2str(maxIter) 'is reached']);
 %     break;
diff --git a/plotGMM.m b/plotGMM.m
index e419dc67aa2bd3850bad519a993fff0f13b1cb3d..14e144ba106f547c1f106335305aabbddcd959ad 100644
--- a/plotGMM.m
+++ b/plotGMM.m
@@ -1,22 +1,22 @@
-function h = plotGMM(Mu, Sigma, color)
-% This function displays the parameters of a Gaussian Mixture Model (GMM).
-% Inputs -----------------------------------------------------------------
-%   o Mu:           D x K array representing the centers of K Gaussians.
-%   o Sigma:        D x D x K array representing the covariance matrices of K Gaussians.
-%   o color:        3 x 1 array representing the RGB color to use for the display. 
-%
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-
-nbStates = size(Mu,2);
-nbDrawingSeg = 35;
-lightcolor = min(color+0.3,1);
-t = linspace(-pi, pi, nbDrawingSeg);
-
-h=[];
-for i=1:nbStates
-  R = real(sqrtm(1.0.*Sigma(:,:,i)));
-  X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg);
-  h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)];
-  h = [h plot(Mu(1,:), Mu(2,:), 'x', 'lineWidth', 2, 'markersize', 6, 'color', color)];
-end
+function h = plotGMM(Mu, Sigma, color)
+% This function displays the parameters of a Gaussian Mixture Model (GMM).
+% Inputs -----------------------------------------------------------------
+%   o Mu:           D x K array representing the centers of K Gaussians.
+%   o Sigma:        D x D x K array representing the covariance matrices of K Gaussians.
+%   o color:        3 x 1 array representing the RGB color to use for the display. 
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+
+nbStates = size(Mu,2);
+nbDrawingSeg = 35;
+lightcolor = min(color+0.3,1);
+t = linspace(-pi, pi, nbDrawingSeg);
+
+h=[];
+for i=1:nbStates
+	R = real(sqrtm(1.0.*Sigma(:,:,i)));
+	X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg);
+	h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)];
+	h = [h plot(Mu(1,:), Mu(2,:), 'x', 'lineWidth', 2, 'markersize', 6, 'color', color)];
+end
diff --git a/reproduction_DS.m b/reproduction_DS.m
index fc4a85a8ef78a097113b01b83fde7f2c749d2b48..a699846f3a6f337de4b47fb831a7845603e05893 100644
--- a/reproduction_DS.m
+++ b/reproduction_DS.m
@@ -1,42 +1,42 @@
-function r = reproduction_DS(DataIn, model, r, currPos)
-% Reproduction with a virtual spring-damper system with constant impedance parameters 
-%
-% Author: Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
-%
-% This source code is given for free! In exchange, I would be grateful if you cite  
-% the following reference in any academic publication that uses this code or part of it: 
-%
-% @inproceedings{Calinon14ICRA,
-%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
-%   title="A task-parameterized probabilistic model with minimal intervention control",
-%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
-%   year="2014",
-%   month="May-June",
-%   address="Hong Kong, China",
-%   pages="3339--3344"
-% }
-
-nbData = size(DataIn,2);
-nbVarOut = model.nbVar - size(DataIn,1);
-
-%% Reproduction with constant impedance parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-x = currPos;
-dx = zeros(nbVarOut,1);
-for t=1:nbData
-  L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; 
-  %Compute acceleration
-  ddx =  -L * [x-r.currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
-  %Update velocity and position
-  dx = dx + ddx * model.dt;
-  x = x + dx * model.dt;
-  %Log data
-  r.Data(:,t) = [DataIn(:,t); x]; 
-  r.ddxNorm(t) = norm(ddx);
-  r.kpDet(t) = det(L(:,1:nbVarOut));
-  r.kvDet(t) = det(L(:,nbVarOut+1:end));
-end
-
-
-
+function r = reproduction_DS(DataIn, model, r, currPos)
+% Reproduction with a virtual spring-damper system with constant impedance parameters 
+%
+% Author: Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+nbData = size(DataIn,2);
+nbVarOut = model.nbVar - size(DataIn,1);
+
+%% Reproduction with constant impedance parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x = currPos;
+dx = zeros(nbVarOut,1);
+for t=1:nbData
+	L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; 
+	%Compute acceleration
+	ddx =  -L * [x-r.currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+	%Update velocity and position
+	dx = dx + ddx * model.dt;
+	x = x + dx * model.dt;
+	%Log data
+	r.Data(:,t) = [DataIn(:,t); x]; 
+	r.ddxNorm(t) = norm(ddx);
+	r.kpDet(t) = det(L(:,1:nbVarOut));
+	r.kvDet(t) = det(L(:,nbVarOut+1:end));
+end
+
+
+