diff --git a/README.md b/README.md
index 214f45e347a4185fe408a529a627da7ff81b39e8..5850342369933d9943f5fa139f6368657328868d 100644
--- a/README.md
+++ b/README.md
@@ -1,51 +1,86 @@
 # pbdlib-matlab
 
+PbDLib is a set of tools combining statistical learning, dynamical systems and optimal control approaches for programming-by-demonstration applications.
+
+The Matlab/GNU Octave version is currently maintained by Sylvain Calinon, http://calinon.ch.
+
+More information can be found on: http://programming-by-demonstration.org/lib/
+
 ### Compatibility
 
-	The codes have been tested with both Matlab and GNU Octave.
+The codes are compatible with both Matlab or GNU Octave.
 
 ### Usage
 
-	Examples starting with 'demo_' can be run from Matlab/GNU Octave.
+Examples starting with `demo_` can be run from Matlab/GNU Octave.
 
 ### References
 
-	Calinon, S. (in preparation). A tutorial on task-parameterized movement learning and retrieval.
-
-	Calinon, S., Bruno, D. and Caldwell, D.G. (2014). A task-parameterized probabilistic model with minimal intervention 
-	control. Proc. of the IEEE Intl Conf. on Robotics and Automation (ICRA).
-
-### Description
-
-	Demonstration of 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.
-
-### Datasets
-	
-	The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within 
-	the context of the AMARSI European Project. Reference: S.M. Khansari-Zadeh and A. Billard, "Learning Stable Non-Linear 
-	Dynamical Systems with Gaussian Mixture Models", IEEE Transaction on Robotics, 2011.
-
-### Contact
-
-	Sylvain Calinon
-	http://programming-by-demonstration.org/lib/
-		
-	This source code is given for free! In exchange, we 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"
-	}
+Did you find PbDLib useful for your research? Please acknowledge the authors in any academic publications that used some parts of these codes.
+
+```
+@article{Calinon15,
+	author="Calinon, S.",
+	title="A tutorial on task-parameterized movement learning and retrieval",
+	year="2015",
+}
+```
+```
+@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"
+}
+```
+
+### Dataset
+
+The folder "data" contains a dataset of 2D handwriting movements from LASA-EPFL (http://lasa.epfl.ch), collected within the context of the AMARSI European Project. Reference: S.M. Khansari-Zadeh and A. Billard, "Learning Stable Non-Linear Dynamical Systems with Gaussian Mixture Models", IEEE Transaction on Robotics, 2011.
+
+### List of examples
+
+All the examples are located in the main folder, and the functions are located in the `m_fcts` folder.
 
+| Filename | Description |
+|----------|-------------|
+| benchmark_DS_GP_GMM01 | Benchmark of task-parameterized model based on Gaussian process regression, with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction |
+| benchmark_DS_GP_raw01 | Benchmark of task-parameterized model based on Gaussian process regression, with raw trajectory, and spring-damper system used for reproduction |
+| benchmark_DS_PGMM01 | Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction |
+| benchmark_DS_TP_GMM01 | Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction |
+| benchmark_DS_TP_GP01 | Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method) |
+| benchmark_DS_TP_LWR01 | Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method) |
+| benchmark_DS_TP_MFA01 | Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction |
+| benchmark_DS_TP_trajGMM01 | Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction |
+| demo_batchLQR01 | Controller retrieval through a batch solution of linear quadratic optimal control (unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_iterativeLQR01) |
+| demo_DSGMR01 | Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction, with decay variable used as input (as in DMP) |
+| demo_DTW01 | Trajectory realignment through dynamic time warping (DTW) |
+| demo_GMM01 | Gaussian mixture model (GMM) parameters estimation |
+| demo_GMR01 | Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction |
+| demo_GPR01 | Use of Gaussian process regression (GPR) as a task-parameterized model, with DS-GMR used to retrieve continuous movements |
+| demo_HDDC01 | High Dimensional Data Clustering (HDDC, or HD-GMM) model from Bouveyron (2007) |
+| demo_iterativeLQR01 | Controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data (see also demo_batchLQR01) |
+| demo_MFA01 | Mixture of factor analyzers (MFA) |
+| demo_MPPCA01 | Mixture of probabilistic principal component analyzers (MPPCA) |
+| demo_stdPGMM01 | Parametric Gaussian mixture model (PGMM) used as a task-parameterized model, with DS-GMR employed to retrieve continuous movements |
+| demo_testLQR01 | Test of the linear quadratic regulation with different variance in the data |
+| demo_testLQR02 | Test of the linear quadratic regulation (LQR) with evaluation of the damping ratio found by the system |
+| demo_testLQR03 | Comparison of linear quadratic regulators (LQR) with finite and infinite time horizons |
+| demo_TPbatchLQR01 | Task-parameterized GMM encoding position and velocity data, combined with a batch solution of linear quadratic optimal control (unconstrained linear MPC) |
+| demo_TPbatchLQR02 | Batch solution of a linear quadratic optimal control (unconstrained linear MPC) acting in multiple frames, which is equivalent to TP-GMM combined with LQR |
+| demo_TPGMM01 | Task-parameterized Gaussian mixture model (TP-GMM) encoding |
+| demo_TPGMR01 | Task-parameterized Gaussian mixture model (TP-GMM), with GMR used for reproduction (without dynamical system) |
+| demo_TPGMR_DS01 | Dynamical system with constant gains used with a task-parameterized model |
+| demo_TPGMR_LQR01 | Finite horizon LQR used with a task-parameterized model  
+| demo_TPGMR_LQR02 | Infinite horizon LQR used with a task-parameterized model  
+| demo_TPGP01 | Task-parameterized Gaussian process regression (TP-GPR) |
+| demo_TPHDDC01 | Task-parameterized high dimensional data clustering (TP-HDDC) |
+| demo_TPMFA01 | Task-parameterized mixture of factor analyzers (TP-MFA), without motion retrieval |
+| demo_TPMPPCA01 | Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) |
+| demo_TPtrajGMM01 | Task-parameterized model with trajectory-GMM encoding |
+| demo_trajGMM01 | Reproduction of trajectory with a GMM with dynamic features (trajectory GMM) |
+| demoIK_nullspace_TPGMM01 | IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames) |
+| demoIK_pointing_TPGMM01 | Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), by using a single Euler orientation angle and 3 DOFs robot) |
diff --git a/benchmark_DS_GP_GMM01.m b/benchmark_DS_GP_GMM01.m
index ab3b9025a7ffa877b1269d37025f8836be4f8e93..5cef271592bbdc620ee63da384064115b440749e 100644
--- a/benchmark_DS_GP_GMM01.m
+++ b/benchmark_DS_GP_GMM01.m
@@ -1,10 +1,22 @@
 function benchmark_DS_GP_GMM01
-%Benchmark of task-parameterized model based on Gaussian process regression, 
-%with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized model based on Gaussian process regression, 
+% with trajectory model (Gaussian mixture model encoding), and DS-GMR used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -14,8 +26,8 @@ model.dt = 0.01; %Time step
 model.kP = 100; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 4; %Number of reproductions with new situations randomly generated
-nbVarOut = model.nbVar-1;
-L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+nbVarOut = model.nbVar-1; %(here: x1,x2)
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
 
 
 %% Load 3rd order tensor data
@@ -25,8 +37,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -40,7 +52,7 @@ 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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Compute data with derivatives
 %Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
 Data = s(1).Data0(1,:);
 for n=1:nbSamples
@@ -102,14 +114,14 @@ for n=1:nbSamples
 	end
 	
 % 	%Retrieval of attractor path through GMR
-% 	currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+% 	currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
 % 	
 % 	%Motion retrieval with spring-damper system
 % 	x = s(n).p(1).b(2:model.nbVar);
 % 	dx = zeros(nbVarOut,1);
 % 	for t=1:s(n).nbData
 % 		%Compute acceleration, velocity and position
-% 		ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+% 		ddx =  -L * [x-currTar(:,t); dx]; 
 % 		dx = dx + ddx * model.dt;
 % 		x = x + dx * model.dt;
 % 		r(n).Data(:,t) = x;
@@ -146,14 +158,14 @@ for n=1:nbRepros
 	end
 	
 	%Retrieval of attractor path through GMR
-	[rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	[rnew(n).currTar, rnew(n).currSigma] = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
 	
 	%Motion retrieval with spring-damper system
 	x = rnew(n).p(1).b(2:model.nbVar);
 	dx = zeros(nbVarOut,1);
 	for t=1:nbD
 		%Compute acceleration, velocity and position
-		ddx =  -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf 
+		ddx =  -L * [x-rnew(n).currTar(:,t); dx]; 
 		dx = dx + ddx * model.dt;
 		x = x + dx * model.dt;
 		rnew(n).Data(:,t) = x;
@@ -180,9 +192,10 @@ for n=1:nbSamples
 	plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_GP_GMM01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -193,8 +206,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_GP_GMM' num2str(n+1,'%.2d') '.png']);
+	pause
 end
 
 pause;
diff --git a/benchmark_DS_GP_raw01.m b/benchmark_DS_GP_raw01.m
index fc04165a065c58d5ebf6781ea036b896a95abad3..32d2966949bad4eca4b44d731b102ddc025fe083 100644
--- a/benchmark_DS_GP_raw01.m
+++ b/benchmark_DS_GP_raw01.m
@@ -1,10 +1,22 @@
 function benchmark_DS_GP_raw01
-%Benchmark of task-parameterized model based on Gaussian process regression, 
-%with raw trajectory, and spring-damper system used for reproduction
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized model based on Gaussian process regression, 
+% with raw trajectory, and spring-damper system used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbFrames = 2; %Number of candidate frames of reference
@@ -15,6 +27,7 @@ model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 4; %Number of reproductions with new situations randomly generated
 L = [eye(model.nbVar)*model.kP, eye(model.nbVar)*model.kV];
 
+
 %% Load 3rd order tensor data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 disp('Load 3rd order tensor data...');
@@ -22,8 +35,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -36,7 +49,7 @@ 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(model.nbVar));
-%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Compute derivatives
 %Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
 Data = s(1).Data0(1,:);
 for n=1:nbSamples
@@ -45,6 +58,7 @@ for n=1:nbSamples
 	Data = [Data; s(n).Data]; %Data is a matrix of size M*D x T (stacking the different trajectory samples)
 end
 
+
 %% GPR with raw trajectory encoding
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 fprintf('Parameters estimation of GPR with raw trajectory encoding:');
@@ -70,7 +84,7 @@ end
 % 	dx = zeros(model.nbVar,1);
 % 	for t=1:s(n).nbData
 % 		%Compute acceleration, velocity and position
-% 		ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+% 		ddx =  -L * [x-currTar(:,t); dx]; 
 % 		dx = dx + ddx * model.dt;
 % 		x = x + dx * model.dt;
 % 		r(n).Data(:,t) = x;
@@ -101,7 +115,7 @@ for n=1:nbRepros
 	dx = zeros(model.nbVar,1);
 	for t=1:nbD
 		%Compute acceleration, velocity and position
-		ddx =  -L * [x-rnew(n).currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf 
+		ddx =  -L * [x-rnew(n).currTar(:,t); dx]; 
 		dx = dx + ddx * model.dt;
 		x = x + dx * model.dt;
 		rnew(n).Data(:,t) = x;
@@ -125,9 +139,10 @@ for n=1:nbSamples
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_GP_raw01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -137,8 +152,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_GP_raw' num2str(n+1,'%.2d') '.png']);
+	pause;
 end
 
 pause;
diff --git a/benchmark_DS_PGMM01.m b/benchmark_DS_PGMM01.m
index 6aaf9df7819cafd0e629d3f2162791627c689ebd..d33b277eeb98c42c3bd94fcaf696829e6816d8ca 100644
--- a/benchmark_DS_PGMM01.m
+++ b/benchmark_DS_PGMM01.m
@@ -1,9 +1,21 @@
 function benchmark_DS_PGMM01
-%Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized model based on parametric Gaussian mixture model, and DS-GMR used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -13,8 +25,8 @@ model.dt = 0.01; %Time step
 model.kP = 100; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 4; %Number of reproductions with new situations randomly generated
-nbVarOut = model.nbVar - 1;
-L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+nbVarOut = model.nbVar-1; %(here, x1,x2)
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
 
 
 %% Load 3rd order tensor data
@@ -24,8 +36,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -38,7 +50,7 @@ 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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Compute derivatives
 Data = [];
 for n=1:nbSamples
 	DataTmp = s(n).Data0(2:end,:);
@@ -51,9 +63,10 @@ end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 fprintf('Parameters estimation of PGMM with EM:');
 for n=1:nbSamples	
-% 	%Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+% 	%Task parameters rearranged as a vector (position and orientation), see Eq. (48) 
 % 	s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1];
-	%Task parameters rearranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+
+	%Task parameters rearranged as a vector (position only), see Eq. (48) 
 	s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1];
 end
 
@@ -61,10 +74,12 @@ end
 % %model = init_GMM_kmeans(Data, model);
 % model = init_GMM_timeBased(Data, model);
 % model = EM_GMM(Data, model);
+
 for i=1:model.nbStates
 % 	%Initialization of parameters based on standard GMM
 % 	model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1));
 % 	model.ZMu(:,end,i) = model.Mu(:,i);
+
 		%Random initialization of parameters
 		model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1));
 		model.Sigma(:,:,i) = eye(model.nbVar);
@@ -83,17 +98,18 @@ nbVarOut = model.nbVar-1;
 for n=1:nbSamples
 	%Computation of the resulting Gaussians (for display purpose)
 	for i=1:model.nbStates
-		model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf
+		model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (48) 
 	end
 	r(n).Mu = model.Mu;
+	
 % 	%Retrieval of attractor path through GMR
-% 	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+% 	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (17)-(19)
 % 	%Motion retrieval with spring-damper system
 % 	x = s(n).p(1).b(2:model.nbVar);
 % 	dx = zeros(nbVarOut,1);
 % 	for t=1:s(n).nbData
 % 		%Compute acceleration, velocity and position
-% 		ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+% 		ddx =  -L * [x-currTar(:,t); dx]; 
 % 		dx = dx + ddx * model.dt;
 % 		x = x + dx * model.dt;
 % 		r(n).Data(:,t) = x;
@@ -110,6 +126,7 @@ for n=1:nbRepros
 	
 % 	%Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
 % 	rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1];
+
 	%Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
 	rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1];
 	
@@ -152,9 +169,10 @@ for n=1:nbSamples
 	plotGMM(r(n).Mu(2:3,:),model.Sigma(2:3,2:3,:), [0 0 0], .04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_PGMM01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -165,8 +183,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_PGMM' num2str(n+1,'%.2d') '.png']);
+	pause;
 end
 
 pause;
diff --git a/benchmark_DS_TP_GMM01.m b/benchmark_DS_TP_GMM01.m
index b0df6d1d95db7246bb0b2a19ba5b97d95f7ff4ab..5a0dc70cbffc41b802ea58d5b82ab695b84220ed 100644
--- a/benchmark_DS_TP_GMM01.m
+++ b/benchmark_DS_TP_GMM01.m
@@ -1,6 +1,17 @@
 function benchmark_DS_TP_GMM01
-%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
@@ -22,8 +33,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -37,7 +48,7 @@ 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, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Create 3rd order tensor data with XHAT instead of X
 Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
 for n=1:nbSamples
 	DataTmp = s(n).Data0(2:end,:);
@@ -97,9 +108,10 @@ for n=1:nbSamples
 	plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_TP_GMM01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -110,8 +122,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_TP_GMM' num2str(n+1,'%.2d') '.png']);
+	pause;
 end
 
 pause;
diff --git a/benchmark_DS_TP_GP01.m b/benchmark_DS_TP_GP01.m
index a0714527dccb791488532a1f0722c9a394f76658..dfc4fe7058342bb2226f6078161d9e91a6b970ee 100644
--- a/benchmark_DS_TP_GP01.m
+++ b/benchmark_DS_TP_GP01.m
@@ -1,9 +1,21 @@
 function benchmark_DS_TP_GP01
-%Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method)
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized Gaussian process (nonparametric task-parameterized method)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbFrames = 2; %Number of candidate frames of reference
@@ -12,8 +24,8 @@ model.dt = 0.01; %Time step
 model.kP = 100; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 4; %Number of reproductions with new situations randomly generated
-nbVarOut = model.nbVar-1;
-L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+nbVarOut = model.nbVar-1; %(here, x1,x2)
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
 
 
 %% Load 3rd order tensor data
@@ -23,8 +35,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -37,7 +49,7 @@ 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(model.nbVar-1));
-%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Create 3rd order tensor data with XHAT instead of X
 Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
 for n=1:nbSamples
 	DataTmp = s(n).Data0(2:end,:);
@@ -64,6 +76,7 @@ for m=1:model.nbFrames
 	model.Mu(:,m,:) = MuTmp;
 	model.Sigma(:,:,m,:) = SigmaTmp;
 end
+
 %Reproduction with spring-damper system
 % for n=1:nbSamples
 % 	currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
@@ -121,9 +134,10 @@ for n=1:nbSamples
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_TP_GP01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -133,8 +147,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_TP_GP' num2str(n+1,'%.2d') '.png']);
+	pause
 end
 
 pause;
diff --git a/benchmark_DS_TP_LWR01.m b/benchmark_DS_TP_LWR01.m
index e7f66818f2c640184bf015a6b5e2c8c09f5b21e3..a6cf26cb59fb344066dc288dda46b3d598e162d2 100644
--- a/benchmark_DS_TP_LWR01.m
+++ b/benchmark_DS_TP_LWR01.m
@@ -1,9 +1,21 @@
 function benchmark_DS_TP_LWR01
-%Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method)
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized locally weighted regression (nonparametric task-parameterized method)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbFrames = 2; %Number of candidate frames of reference
@@ -12,8 +24,8 @@ model.dt = 0.01; %Time step
 model.kP = 100; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 4; %Number of reproductions with new situations randomly generated
-nbVarOut = model.nbVar-1;
-L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+nbVarOut = model.nbVar-1; %(here, x1,x2)
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback gains
 
 
 %% Load 3rd order tensor data
@@ -23,8 +35,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -37,7 +49,7 @@ 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(model.nbVar-1));
-%Create 3rd order tensor data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Compute derivatives
 Data = zeros(model.nbVar, model.nbFrames, nbD*nbSamples);
 for n=1:nbSamples
 	DataTmp = s(n).Data0(2:end,:);
@@ -75,6 +87,7 @@ for m=1:model.nbFrames
 	model.Mu(:,m,:) = MuTmp;
 	model.Sigma(:,:,m,:) = SigmaTmp;
 end
+
 %Reproduction with spring-damper system
 % for n=1:nbSamples
 % 	currTar = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
@@ -132,9 +145,10 @@ for n=1:nbSamples
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_TP_LWR01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -144,8 +158,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_TP_LWR' num2str(n+1,'%.2d') '.png']);
+	pause;
 end
 
 pause;
diff --git a/benchmark_DS_TP_MFA01.m b/benchmark_DS_TP_MFA01.m
index 92be712e06a149f6b8a2f9c280ec5d31dfb196d0..1f06468d68165ff459761124440646ff7c0dc3df 100644
--- a/benchmark_DS_TP_MFA01.m
+++ b/benchmark_DS_TP_MFA01.m
@@ -1,9 +1,21 @@
 function benchmark_DS_TP_MFA01
-%Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized mixture of factor analyzers (TP-MFA), with DS-GMR used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -98,9 +110,10 @@ for n=1:nbSamples
 	plotGMM(r(n).Mu(2:3,:),r(n).Sigma(2:3,2:3,:), [0 0 0], .04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_TP_MFA01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_TP_MFA01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -111,8 +124,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_TP_MFA' num2str(n+1,'%.2d') '.png']);
+	pause;
 end
 
 pause;
diff --git a/benchmark_DS_TP_trajGMM01.m b/benchmark_DS_TP_trajGMM01.m
index 9078fe8e876797b1d950c3dfb63a3e6fda2d4add..84053ddd25f5f1b3fbcc07d63f22c19995956412 100644
--- a/benchmark_DS_TP_trajGMM01.m
+++ b/benchmark_DS_TP_trajGMM01.m
@@ -1,20 +1,32 @@
 function benchmark_DS_TP_trajGMM01
-%Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS-GMR used for reproduction
-%Sylvain Calinon, 2015
+% Benchmark of task-parameterized Gaussian mixture model (TP-GMM), with DS used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
 model.nbFrames = 2; %Number of candidate frames of reference
 model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
 model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
-model.dt = .1; %Time step
+model.dt = 0.1; %Time step
 model.kP = 10; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 4; %Number of reproductions with new situations randomly generated
-L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV];
+L = [eye(model.nbVarPos)*model.kP, eye(model.nbVarPos)*model.kV]; %Feedback gains
 
 
 %% Load 3rd order tensor data
@@ -24,8 +36,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -44,10 +56,6 @@ for n=1:nbSamples
 	s(n).Data = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
 end
 
-% for n=1:nbSamples
-% 	s(n).Data = s(n).Data0;
-% end
-
 
 %% Compute derivatives
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -215,9 +223,10 @@ for n=1:nbSamples
 	plotGMM(r(n).Mu(1:2,:),r(n).Sigma(1:2,1:2,:), [0 0 0], .04);
 end
 axis equal; axis(limAxes);
-print('-dpng','-r600','graphs/benchmark_DS_TP_trajGMM01.png');
+%print('-dpng','-r600','graphs/benchmark_DS_TP_trajGMM01.png');
 
 %Plot reproductions in new situations
+disp('[Press enter to see next reproduction attempt]');
 h=[];
 for n=1:nbRepros
 	delete(h);
@@ -228,8 +237,8 @@ for n=1:nbRepros
 		[1 1 1],'linewidth',1.5,'edgecolor',[0 0 0],'facealpha',0,'edgealpha',0.4)];
 	h = [h plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[0 0 0])];
 	axis equal; axis(limAxes);
-	print('-dpng','-r600',['graphs/benchmark_DS_TP_trajGMM' num2str(n+1,'%.2d') '.png']);
-	%pause
+	%print('-dpng','-r600',['graphs/benchmark_DS_TP_trajGMM' num2str(n+1,'%.2d') '.png']);
+	pause;
 end
 
 pause;
diff --git a/data/DataWithDeriv01.mat b/data/DataWithDeriv01.mat
new file mode 100644
index 0000000000000000000000000000000000000000..397d036b1ce870343889574d148547f95f6e6453
Binary files /dev/null and b/data/DataWithDeriv01.mat differ
diff --git a/data/DataWithDeriv02.mat b/data/DataWithDeriv02.mat
new file mode 100644
index 0000000000000000000000000000000000000000..f8326ba6c8c6da8e7821d93698c69b4ea0a60cba
Binary files /dev/null and b/data/DataWithDeriv02.mat differ
diff --git a/data/TPGMMpointing_nullspace_data02.mat b/data/TPGMMpointing_nullspace_data02.mat
index ec8e57e3ca33d94c3b66019d2756614fb7a5af63..9c69bb66915d7d6ae660fe58eba8a33ba3a54715 100644
Binary files a/data/TPGMMpointing_nullspace_data02.mat and b/data/TPGMMpointing_nullspace_data02.mat differ
diff --git a/data/TPGMMpointing_nullspace_model02.mat b/data/TPGMMpointing_nullspace_model02.mat
index 3de1f30862e922a00fef5da5ddf7917081c7e6f3..eb756ca7020afe5db1e2653c03769bc4bdb5965d 100644
Binary files a/data/TPGMMpointing_nullspace_model02.mat and b/data/TPGMMpointing_nullspace_model02.mat differ
diff --git a/data/TPGMMpointing_nullspace_repro02.mat b/data/TPGMMpointing_nullspace_repro02.mat
index 0c1cb16c33353933c22864c7ff9fc71018da3972..61131fa9078f2d8582547583ddc1e39ffb0d5d8b 100644
Binary files a/data/TPGMMpointing_nullspace_repro02.mat and b/data/TPGMMpointing_nullspace_repro02.mat differ
diff --git a/data/TPGMMtmp.mat b/data/TPGMMtmp.mat
index 157c0378aaee45aa2246073737837c9838daeb20..2cdd77e752fcee51fe871bd123ade07bfd463b57 100644
Binary files a/data/TPGMMtmp.mat and b/data/TPGMMtmp.mat differ
diff --git a/demoIK_nullspace_TPGMM01.m b/demoIK_nullspace_TPGMM01.m
index 01ea7fa0058eae3bd2182080c1236b5d2b52e573..380d98972d5a4b88c9394176ddc405f1327551fd 100644
--- a/demoIK_nullspace_TPGMM01.m
+++ b/demoIK_nullspace_TPGMM01.m
@@ -1,10 +1,23 @@
 function demoIK_nullspace_TPGMM01
-%IK with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames)
-%First run 'startup_rvc' from the robotics toolbox
-%Sylvain Calinon, 2015
+% Inverse kinematics with nullspace treated with task-parameterized GMM (bimanual tracking task, version with 4 frames).
+%
+% This example requires Peter Corke's robotics toolbox (run 'startup_rvc' from the robotics toolbox).
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 1; %Number of states
@@ -22,12 +35,14 @@ pinvDampCoeff = 1e-8; %Coefficient for damped pseudoinverse
 
 needsModel = 1;
 
+
 %% Create robot
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 armLength = .5;
 L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
 arm = SerialLink(repmat(L1,3,1));
 
+
 %% Generate demonstrations 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 if needsModel==1
@@ -46,6 +61,9 @@ for n=1:nbSamples
 			%Objects moving on a line
 			s(n).lxh = [linspace(s(n).lx(1,1),s(n).lx(1,1)-.6*armLength,nbData); linspace(s(n).lx(2,1),s(n).lx(2,1)+2*armLength,nbData)];
 			s(n).rxh = [linspace(s(n).rx(1,1),s(n).rx(1,1)+.6*armLength,nbData); linspace(s(n).rx(2,1),s(n).rx(2,1)+2*armLength,nbData)];
+% 			%Objects moving on a curve
+% 			s(n).lxh = [-sin(linspace(0,pi,nbData))*0.4+s(n).lx(1,1); linspace(s(n).lx(2,1),s(n).lx(2,1)+2*armLength,nbData)];
+% 			s(n).rxh = [sin(linspace(0,pi,nbData))*0.4+s(n).rx(1,1); linspace(s(n).rx(2,1),s(n).rx(2,1)+2*armLength,nbData)];
 		end
 		%Build Jacobians
 		lJ = arm.jacob0(q(1:3),'trans');
@@ -116,7 +134,7 @@ model = init_TPGMM_timeBased(Data, model); %Initialization
 %model = init_TPGMM_kmeans(Data, model); %Initialization
 model = EM_TPGMM(Data, model);
 
-model.nbVar = model.nbQ; %Update nbVar to use productTPGMM()
+model.nbVar = model.nbQ; %Update of nbVar to later use productTPGMM()
 
 
 %% Reproduction 
@@ -137,6 +155,9 @@ for n=1:nbRepros
 			%Objects moving on a line
 			r(n).lxh = [linspace(r(n).lx(1,1),r(n).lx(1,1)-.6*armLength,nbData); linspace(r(n).lx(2,1),r(n).lx(2,1)+2*armLength,nbData)];
 			r(n).rxh = [linspace(r(n).rx(1,1),r(n).rx(1,1)+.6*armLength,nbData); linspace(r(n).rx(2,1),r(n).rx(2,1)+2*armLength,nbData)];
+% 			%Objects moving on a curve
+% 			r(n).lxh = [-sin(linspace(0,pi,nbData))*0.3+r(n).lx(1,1); linspace(r(n).lx(2,1),r(n).lx(2,1)+2*armLength,nbData)];
+% 			r(n).rxh = [sin(linspace(0,pi,nbData))*0.3+r(n).rx(1,1); linspace(r(n).rx(2,1),r(n).rx(2,1)+2*armLength,nbData)];
 		end
 		%IK controller
 		ldx = (r(n).lxh(:,t) - r(n).lx(:,t)) / model.dt;
@@ -198,6 +219,7 @@ save('data/TPGMMtmp.mat','s','r','model');
 end %needsModel
 load('data/TPGMMtmp.mat');
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[10,10,1000,450],'color',[1 1 1]); 
diff --git a/demoIK_pointing_nullspace_TPGMM01.m b/demoIK_pointing_TPGMM01.m
similarity index 89%
rename from demoIK_pointing_nullspace_TPGMM01.m
rename to demoIK_pointing_TPGMM01.m
index 720e38ba718f7e2f1ddbb65bbf5ff98c00d46681..cbf56d00163b6300754d5e25d2718b903dcf2164 100644
--- a/demoIK_pointing_nullspace_TPGMM01.m
+++ b/demoIK_pointing_TPGMM01.m
@@ -1,12 +1,25 @@
-function demoIK_pointing_nullspace_TPGMM01
-%Example of task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) 
-%(example with two objects and robot frame, starting from the same initial pose (nullspace constraint), 
-%by using a single Euler orientation angle and 3 DOFs robot)
-%This example requires 'startup_rvc' to be run from the Robotics Toolbox.
-%Sylvain Calinon, 2015
+function demoIK_pointing_TPGMM01
+% Task-parameterized GMM to encode pointing direction by considering nullspace constraint (4 frames) 
+% (example with two objects and robot frame, starting from the same initial pose (nullspace constraint), 
+% by using a single Euler orientation angle and 3 DOFs robot)
+%
+% This example requires Peter Corke's Robotics Toolbox (run 'startup_rvc' from the Robotics Toolbox).
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 6; %Number of states in the GMM
@@ -24,9 +37,10 @@ eMax = 1; %Maximum error norm for stable Jacobian computation
 Kp = 0.15; %Amplification gain for error computation 
 KpQ = 0.15; %Amplification gain for joint angle error computation 
 
-needsData = 0;
-needsModel = 0;
-needsRepro = 0;
+needsData = 1;
+needsModel = 1;
+needsRepro = 1;
+
 
 %% Create robot (requires the Robotics Toolbox)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -317,7 +331,7 @@ for n=1:nbRepros
 	end
 end
 
-%print('-dpng','graphs/demoIK_pointing_nullspace_TPGMM01.png');
+%print('-dpng','graphs/demoIK_pointing_TPGMM01.png');
 %pause;
 %close all;
 
diff --git a/demo_DSGMR01.m b/demo_DSGMR01.m
index 350709f648a681966e174722e43adcf3264a1663..d7a415e23e3a8b6920d49c6d944fa2a9c50374b5 100644
--- a/demo_DSGMR01.m
+++ b/demo_DSGMR01.m
@@ -1,23 +1,35 @@
 function demo_DSGMR01
-%Example of Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction,
-%with decay variable used as input (as in DMP)
-%Sylvain Calinon, 2015
+% Gaussian mixture model (GMM), with Gaussian mixture regression(GMR) and dynamical systems used for reproduction,
+% with decay variable used as input (as in DMP).
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 5; %Number of states in the GMM
 model.nbVar = 3; %Number of variables [t,x1,x2]
 nbData = 200; %Length of each trajectory
-model.dt = 0.001; %Time step
+model.dt = 0.01; %Time step
 nbSamples = 5; %Number of demonstrations
-
-model.kP = 2000; %Stiffness gain
+model.kP = 50; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 model.alpha = 1.0; %Decay factor
-nbVarOut = model.nbVar-1;
-L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
+nbVarOut = model.nbVar-1; %Dimension of spatial variables
+L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV]; %Feedback term
+
 
 %% Load AMARSI data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -25,14 +37,21 @@ sIn(1) = 1; %Initialization of decay term
 for t=2:nbData
 	sIn(t) = sIn(t-1) - model.alpha * sIn(t-1) * model.dt; %Update of decay term (ds/dt=-alpha s)
 end
+% %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 transformation matrix to compute [X; DX; DDX]
+% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt;
+% D(end,end) = 0;
 
-%Store data as [s; x]
-demos=[];
-load('data/AMARSI/CShape.mat');
-Data=[];
+load('data/AMARSI/GShape.mat');
+Data=[]; Data0=[];
 for n=1:nbSamples
-	s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
-	Data = [Data [sIn; s(n).Data]]; 
+	DataTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+	%s(n).Data = K * [DataTmp; DataTmp*D; DataTmp*D*D];
+	s(n).Data = DataTmp;
+	Data = [Data [sIn; s(n).Data]]; %Training data as [s;xhat]
+	Data0 = [Data0 [sIn; DataTmp]]; %Original data as [s;x]
 end
 
 
@@ -41,34 +60,35 @@ end
 %model = init_GMM_timeBased(Data, model);
 model = init_GMM_kmeans(Data, model);
 model = EM_GMM(Data, model);
-[currTar, currSigma] = GMR(model, sIn, 1, 2:3);
+[currTar, currSigma] = GMR(model, sIn, 1, 2:model.nbVar); %see Eq. (17)-(19)
 %Motion retrieval with spring-damper system
 x = Data(2:model.nbVar,1);
 dx = zeros(nbVarOut,1);
 for t=1:nbData
 	%Compute acceleration, velocity and position
-	ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf 
+	ddx = L * [currTar(:,t)-x; -dx]; 
 	dx = dx + ddx * model.dt;
 	x = x + dx * model.dt;
 	DataOut(:,t) = x;
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[10,10,1000,500]); 
 %Plot GMM
 subplot(1,2,1); hold on; box on; title('GMM');
 plotGMM(model.Mu(2:model.nbVar,:), model.Sigma(2:model.nbVar,2:model.nbVar,:), [.8 0 0]);
-plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
+plot(Data0(2,:),Data0(3,:),'.','markersize',8,'color',[.7 .7 .7]);
 axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
 %Plot DS-GMR
 subplot(1,2,2); hold on; box on; title('DS-GMR');
 plotGMM(currTar, currSigma, [0 .8 0]);
-plot(Data(2,:),Data(3,:),'.','markersize',8,'color',[.7 .7 .7]);
+plot(Data0(2,:),Data0(3,:),'.','markersize',8,'color',[.7 .7 .7]);
 plot(currTar(1,:),currTar(2,:),'-','linewidth',1.5,'color',[0 .6 0]);
 plot(DataOut(1,:),DataOut(2,:),'-','linewidth',3,'color',[0 .3 0]);
 axis equal; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
 
 %print('-dpng','graphs/demo_DSGMR01.png');
-%pause;
-%close all;
+pause;
+close all;
diff --git a/demo_DTW01.m b/demo_DTW01.m
new file mode 100644
index 0000000000000000000000000000000000000000..e324bc24570bf9c1cb72fa7716923acad47b78bd
--- /dev/null
+++ b/demo_DTW01.m
@@ -0,0 +1,76 @@
+function demo_DTW01
+% Trajectory realignment through dynamic time warping (DTW).
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbData = 200; %Length of each trajectory
+wMax = 50; %Warping time window 
+nbSamples = 5; %Number of demonstrations
+nbVar = 2; %Number of dimensions (max 2 for AMARSI data)
+
+
+%% Load AMARSI handwriting data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
+load('data/AMARSI/GShape.mat');
+for n=1:nbSamples
+	s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos(1:nbVar,:), linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+end
+
+
+%% Dynamic time warping
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+r(1).Data = s(1).Data;
+for n=2:nbSamples
+	[r(1).Data, r(n).Data, r(n-1).wPath] = DTW(r(1).Data, s(n).Data, wMax);
+	%Realign previous trajectories
+	p = r(n-1).wPath(1,:);
+	for m=2:n-1
+		DataTmp = r(m).Data(:,p);
+		r(m).Data = spline(1:size(DataTmp,2), DataTmp, linspace(1,size(DataTmp,2),nbData)); %Resampling
+	end
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10,10,1000,500]); 
+for k=1:nbVar
+	subplot(2,nbVar,(k-1)*2+1); hold on; if k==1 title('Before DTW'); end;
+	for n=1:nbSamples
+		plot(s(n).Data(k,:), '-','linewidth',1,'color',[0 0 0]);
+	end
+	xlabel('t'); ylabel(['x_' num2str(k)]);
+	axis tight; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+	subplot(2,nbVar,(k-1)*2+2); hold on; if k==1 title('After DTW'); end;
+	for n=1:nbSamples
+		plot(r(n).Data(k,:), '-','linewidth',1,'color',[0 0 0]);
+	end
+	xlabel('t'); ylabel(['x_' num2str(k)]);
+	axis tight; set(gca,'Xtick',[]); set(gca,'Ytick',[]);
+end
+
+figure; hold on;
+for n=1:nbSamples-1
+	plot(r(n).wPath(1,:),r(n).wPath(2,:),'-','color',[0 0 0]);
+end
+xlabel('w_1'); ylabel('w_2');
+
+%print('-dpng','graphs/demo_DTW01.png');
+%pause;
+%close all;
diff --git a/demo_GMM01.m b/demo_GMM01.m
index 11aade306899848221683732fbcde5e69a0f0145..551e647bd895aa9c20e57491fb391eca593254ee 100644
--- a/demo_GMM01.m
+++ b/demo_GMM01.m
@@ -1,9 +1,21 @@
 function demo_GMM01
-%Example of Gaussian mixture model (GMM)
-%Sylvain Calinon, 2015
+% Gaussian mixture model (GMM) parameters estimation.
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 5; %Number of states in the GMM
diff --git a/demo_GMR01.m b/demo_GMR01.m
index afe5c267e73d0b5313110b05183918afd7bdb9e9..bc1e27cbc644233fd1e6fdc94900c7b30578d727 100644
--- a/demo_GMR01.m
+++ b/demo_GMR01.m
@@ -1,9 +1,21 @@
 function demo_GMR01
-%Example of Gaussian mixture model (GMM) and time-based Gaussian mixture regression (GMR) used for reproduction
-%Sylvain Calinon, 2015
+% Gaussian mixture model (GMM) with time-based Gaussian mixture regression (GMR) used for reproduction
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 5; %Number of states in the GMM
@@ -12,7 +24,8 @@ model.dt = 0.001; %Time step duration
 nbData = 200; %Length of each trajectory
 nbSamples = 5; %Number of demonstrations
 
-%% Load AMARSI data
+
+%% Load AMARSI handwriting data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 demos=[];
 load('data/AMARSI/CShape.mat');
@@ -28,7 +41,7 @@ end
 %model = init_GMM_kmeans(Data, model);
 model = init_GMM_timeBased(Data, model);
 model = EM_GMM(Data, model);
-[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:3);
+[DataOut, SigmaOut] = GMR(model, [1:nbData]*model.dt, 1, 2:3); %see Eq. (17)-(19)
 
 
 %% Plots
diff --git a/demo_GPR01.m b/demo_GPR01.m
index 67670111af308af19bed0141ab4689233889a108..b981b5e07eb97414a190300a3931ba51961cd797 100644
--- a/demo_GPR01.m
+++ b/demo_GPR01.m
@@ -1,10 +1,21 @@
 function demo_GPR01
-%Example of Gaussian processs regression (GPR) used as a task-parameterized model,
-%with DS-GMR employed to retrieve continuous movements
-%Sylvain Calinon, 2015
+% Gaussian processs regression (GPR) used for task adaptation, with DS-GMR employed to retrieve continuous movements
+% 
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -80,7 +91,7 @@ DataIn = [1:s(1).nbData] * model.dt;
 nbVarOut = model.nbVar-1;
 L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
 for n=1:nbSamples
-	%Rebuild model parameters with GPR
+	%Rebuild model parameters with GPR, see Eq. (46)
 	vOut = GPR(model.DataIn, model.DataOut, s(n).DataIn);
 	
 	%Re-arrange GPR output as GMM parameters
@@ -95,15 +106,15 @@ for n=1:nbSamples
 		r(n).Sigma(:,:,i) = V * max(D,5E-4) * V';
 	end
 	
-	%Retrieval of attractor path through GMR
-	currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	%Retrieval of attractor path through GMR, see Eq. (17)-(19)
+	currTar = GMR(r(n), DataIn, 1, [2:model.nbVar]); 
 	
 	%Motion retrieval with spring-damper system
 	x = s(n).p(1).b(2:model.nbVar);
 	dx = zeros(nbVarOut,1);
 	for t=1:s(n).nbData
 		%Compute acceleration, velocity and position
-		ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+		ddx =  -L * [x-currTar(:,t); dx]; 
 		dx = dx + ddx * model.dt;
 		x = x + dx * model.dt;
 		r(n).Data(:,t) = x;
@@ -140,15 +151,15 @@ for n=1:nbRepros
 		rnew(n).Sigma(:,:,i) = V * max(D,1E-3) * V';
 	end
 	
-	%Retrieval of attractor path through GMR
-	currTar = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	%Retrieval of attractor path through GMR, see Eq. (17)-(19)
+	currTar = GMR(rnew(n), DataIn, 1, [2:model.nbVar]); 
 	
 	%Motion retrieval with spring-damper system
 	x = rnew(n).p(1).b(2:model.nbVar);
 	dx = zeros(nbVarOut,1);
 	for t=1:nbD
 		%Compute acceleration, velocity and position
-		ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf 
+		ddx =  -L * [x-currTar(:,t); dx]; 
 		dx = dx + ddx * model.dt;
 		x = x + dx * model.dt;
 		rnew(n).Data(:,t) = x;
diff --git a/demo_HDDC01.m b/demo_HDDC01.m
index 358afbe14106f8ac957d4f9a03c33c67a09d65c8..0139babfe678dffad56a0ca66c755550944c2843 100644
--- a/demo_HDDC01.m
+++ b/demo_HDDC01.m
@@ -1,9 +1,21 @@
 function demo_HDDC01
-%Example of High Dimensional Data Clustering (HDDC, or HD-GMM) model from Bouveyron (2007)
-%Sylvain Calinon, 2015
+% Example of High Dimensional Data Clustering (HDDC, or HD-GMM) encoding from Bouveyron (2007)
+% 
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 4; %Number of states in the GMM
@@ -13,7 +25,7 @@ nbData = 200; %Length of each trajectory
 nbSamples = 5; %Number of demonstrations
 
 
-%% Load AMARSI data
+%% Load AMARSI handwriting data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 demos=[];
 load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
@@ -33,7 +45,8 @@ end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model = init_GMM_kmeans(Data, model);
 model0 = EM_GMM(Data, model); %for comparison
-model = EM_HDGMM(Data, model);
+model = EM_HDGMM(Data, model); 
+
 
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
diff --git a/demo_MFA01.m b/demo_MFA01.m
index f1f575c9cf6cc4f1965b988062132b49b331afd9..a9ca0913a2f86bd1d7d972b301bae7dcdcdf0b57 100644
--- a/demo_MFA01.m
+++ b/demo_MFA01.m
@@ -1,9 +1,21 @@
 function demo_MFA01
-%Example of mixture of factor analysers (MFA) 
-%Sylvain Calinon, 2015
+% Mixture of factor analysers (MFA) encoding
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 4; %Number of states in the GMM
@@ -13,7 +25,7 @@ nbData = 200; %Length of each trajectory
 nbSamples = 5; %Number of demonstrations
 
 
-%% Load AMARSI data
+%% Load AMARSI handwriting data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 demos=[];
 load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
@@ -33,7 +45,7 @@ end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model = init_GMM_kmeans(Data, model);
 model0 = EM_GMM(Data, model); %for comparison
-model = EM_MFA(Data, model);
+model = EM_MFA(Data, model); 
 
 
 %% Plots
diff --git a/demo_MPPCA01.m b/demo_MPPCA01.m
index 973ea06fd193fb9ea03f9ce1efbd6b875000eb28..5106a1bb29b3bbec3750dafd6c2af3a753254497 100644
--- a/demo_MPPCA01.m
+++ b/demo_MPPCA01.m
@@ -1,6 +1,17 @@
 function demo_MPPCA01
-%Example of mixture of probabilistic principal component analyzers (MPPCA)
-%Sylvain Calinon, 2015
+% Example of mixture of probabilistic principal component analyzers (MPPCA) encoding
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
@@ -13,7 +24,7 @@ nbData = 200; %Length of each trajectory
 nbSamples = 5; %Number of demonstrations
 
 
-%% Load AMARSI data
+%% Load AMARSI handwriting data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 demos=[];
 load('data/AMARSI/GShape.mat'); %Load x1,x2 variables
diff --git a/demo_TPGMM01.m b/demo_TPGMM01.m
index c92750c904c73f984f23ba0e2d2c4dfb4bbcdf09..37b3fd7b253b4a5e20ac9cbce4cfdc0b365c8578 100644
--- a/demo_TPGMM01.m
+++ b/demo_TPGMM01.m
@@ -1,9 +1,21 @@
 function demo_TPGMM01
-%Example of task-parameterized Gaussian mixture model (TP-GMM)
-%Sylvain Calinon, 2015
+% Task-parameterized Gaussian mixture model (TP-GMM) encoding
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -18,8 +30,8 @@ disp('Load 3rd order tensor data...');
 % 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=2 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 (nbSamples=5).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/Data01.mat');
 
 
@@ -72,6 +84,30 @@ for m=1:model.nbFrames
 	axis square; set(gca,'xtick',[0],'ytick',[0]);
 end
 
+
+% %% Saving of data for C++ version of pbdlib
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% %Save model parameters
+% for m=1:model.nbFrames
+% 	M = [];
+% 	N = [];
+% 	for i=1:model.nbStates
+% 		M = [M model.Sigma(:,:,m,i)];
+% 		N = [N model.Mu(:,m,i)];
+% 	end
+% 	save(['data/Sigma' num2str(m) '.txt'], 'M', '-ascii');
+% 	save(['data/Mu' num2str(m) '.txt'], 'N', '-ascii');
+% end
+% save(['data/Priors.txt'], 'model.Priors', '-ascii');
+% TPGMMparams = [model.nbVar; model.nbFrames; model.nbStates; model.dt];
+% save(['data/Tpgmm.txt'], 'TPGMMparams', '-ascii');
+% %Save task parameters
+% for m=1:model.nbFrames
+% 	Ab = [r(1).p(m).b'; r(1).p(m).A];
+% 	save(['data/Params' num2str(m) '.txt'], 'Ab', '-ascii');
+% end
+
+
 %print('-dpng','graphs/demo_TPGMM01.png');
 %pause;
 %close all;
diff --git a/demo_TPGMR01.m b/demo_TPGMR01.m
index b0e10214e32404ab4506aff157ae639e32a2d26c..f253007dc6a07bc81b4aa72ab2b0f3d81ed74477 100644
--- a/demo_TPGMR01.m
+++ b/demo_TPGMR01.m
@@ -1,9 +1,21 @@
 function demo_TPGMR01
-%Example of task-parameterized Gaussian mixture model (TP-GMM), with GMR used for reproduction 
-%Sylvain Calinon, 2015
+% Task-parameterized Gaussian mixture model (TP-GMM), with time-based GMR used for reproduction 
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...');
 % 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 (nbSamples=5).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/Data02.mat');
 
 
@@ -37,10 +49,10 @@ model = EM_tensorGMM(Data, model);
 disp('Reproductions with GMR...');
 DataIn = s(1).Data(1,:);
 for n=1:nbSamples
-	[r(n).Mu, r(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
+	[r(n).Mu, r(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6)
 	r(n).Priors = model.Priors;
 	r(n).nbStates = model.nbStates;
-	r(n).Data = [DataIn; GMR(r(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	r(n).Data = [DataIn; GMR(r(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (17)-(19)
 end
 
 
@@ -56,10 +68,10 @@ for n=1:nbRepros
 		rnew(n).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
-	[rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
+	[rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p); %See Eq. (6)
 	rnew(n).Priors = model.Priors;
 	rnew(n).nbStates = model.nbStates;
-	rnew(n).Data = [DataIn; GMR(rnew(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	rnew(n).Data = [DataIn; GMR(rnew(n), DataIn, 1, 2:model.nbVar)]; %See Eq. (17)-(19)
 end
 
 
diff --git a/demo_TPGMR_DS01.m b/demo_TPGMR_DS01.m
index 1d6a20fac2b99bbee1bea22f92697582ac55e2bf..1c45fe78473987e0f971e9d68d924669540d9abf 100644
--- a/demo_TPGMR_DS01.m
+++ b/demo_TPGMR_DS01.m
@@ -5,10 +5,11 @@ function demo_TPGMR_DS01
 % 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.
+% This example presents the results for a time-based GMR reference retrieval process combined with a tracking system 
+% with constant gains.
 %
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
 %
 % 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:
@@ -25,6 +26,7 @@ function demo_TPGMR_DS01
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -35,6 +37,7 @@ model.kP = 100; %Stiffness gain
 model.kV = (2*model.kP)^.5; %Damping gain (with ideal underdamped damping ratio)
 nbRepros = 8; %Number of reproductions with new situations randomly generated
 
+
 %% Load 3rd order tensor data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 disp('Load 3rd order tensor data...');
@@ -42,8 +45,8 @@ disp('Load 3rd order tensor data...');
 % 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).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
diff --git a/demo_TPGMR_LQR01.m b/demo_TPGMR_LQR01.m
index 0ad618841e51dabb83c83b0c856f13ed7f580514..70b5025e2615d36506634ae8afe6104945804480 100644
--- a/demo_TPGMR_LQR01.m
+++ b/demo_TPGMR_LQR01.m
@@ -7,10 +7,10 @@ function demo_TPGMR_LQR01
 % 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.
+% This example presents the results for a time-based GMR reference retrieval process combined with a finite horizon LQR.
 %
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
 %
 % 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:
@@ -27,6 +27,7 @@ function demo_TPGMR_LQR01
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -63,7 +64,8 @@ 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);
+	r(n) = reproduction_LQR_finiteHorizon(model, a(n), a(n).currTar(:,1), rFactor);
+	r(n).Data = [DataIn; r(n).Data];
 end
 
 
@@ -80,7 +82,8 @@ for n=1:nbRepros
 	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);
+	rnew(n) = reproduction_LQR_finiteHorizon(model, anew(n), anew(n).currTar(:,1), rFactor);
+	rnew(n).Data = [DataIn; rnew(n).Data];
 end
 
 
diff --git a/demo_TPGMR_LQR02.m b/demo_TPGMR_LQR02.m
index 4f733c716555361efc553179ba89cd753295f973..05294a1057d0a2da7e152e6fe1dd997d093dc5f7 100644
--- a/demo_TPGMR_LQR02.m
+++ b/demo_TPGMR_LQR02.m
@@ -7,10 +7,10 @@ function demo_TPGMR_LQR02
 % 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.
+% This example presents the results for a time-based GMR reference retrieval process combined with an infinite horizon LQR.
 %
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
 %
 % 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:
@@ -27,6 +27,7 @@ function demo_TPGMR_LQR02
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -63,7 +64,8 @@ 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);
+	r(n) = reproduction_LQR_infiniteHorizon(model, a(n), a(n).currTar(:,1), rFactor);
+	r(n).Data = [DataIn; r(n).Data];
 end
 
 
@@ -80,7 +82,8 @@ for n=1:nbRepros
 	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);
+	rnew(n) = reproduction_LQR_infiniteHorizon(model, anew(n), anew(n).currTar(:,1), rFactor);
+	rnew(n).Data = [DataIn; rnew(n).Data];
 end
 
 
diff --git a/demo_TPGP01.m b/demo_TPGP01.m
new file mode 100644
index 0000000000000000000000000000000000000000..a2b17933b61c9faa3b9869223bf401d711545056
--- /dev/null
+++ b/demo_TPGP01.m
@@ -0,0 +1,135 @@
+function demo_TPGP01
+% Task-parameterized Gaussian process regression (TP-GPR)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: x1,x2)
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+nbData = 200; %Number of datapoints in a trajectory
+
+
+%% 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=2 the dimension of a
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
+load('data/Data01.mat');
+
+
+%% Learning (pre-computation)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Learning...');
+model.nbStates = nbData;
+model.Priors = ones(model.nbStates,1) / model.nbStates;
+DataIn(1,:) = repmat(1:nbData,1,nbSamples); 
+for m=1:model.nbFrames
+	DataOut = squeeze(Data(:,m,:));
+	[MuTmp, SigmaTmp] = GPR(DataIn, DataOut, DataIn, [1, 1E1, 1E-1]);
+	model.Mu(:,m,:) = MuTmp;
+	model.Sigma(:,:,m,:) = SigmaTmp;
+end
+
+
+%% Reproduction for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with TP-GP...');
+for n=1:nbSamples
+	r(n).Data = productTPGMM0(model, s(n).p); %See Eq. (6)
+end
+
+
+%% Reproduction for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with TP-GP...');
+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);
+		rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+		rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+	end
+	rnew(n).Data = productTPGMM0(model, rnew(n).p); %See Eq. (6)
+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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','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');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+end
+for n=1:nbSamples
+	%Plot trajectories
+	plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(r(n).Data(1,:), r(n).Data(2,:),'-','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');
+for n=1:nbRepros
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+end
+for n=1:nbRepros
+	%Plot trajectories
+	plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]);
+	plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_TPGP01.png');
+%pause;
+%close all;
+
+
diff --git a/demo_TPHDDC01.m b/demo_TPHDDC01.m
index 9dab570218fc6db9ea366af841e6a8c4e0f7eed5..7db387c26148aef2c807af67708df96e7a4cde29 100644
--- a/demo_TPHDDC01.m
+++ b/demo_TPHDDC01.m
@@ -1,9 +1,21 @@
 function demo_TPHDDC01
-%Example of task-parameterized high dimensional data clustering (TP-HDDC)
-%Sylvain Calinon, 2015
+% Task-parameterized high dimensional data clustering (TP-HDDC)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...');
 % 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=2 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 (nbSamples=5).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/Data01.mat');
 
 
@@ -32,9 +44,10 @@ model = EM_tensorHDGMM(Data, model);
 
 %Reconstruct GMM for each demonstration
 for n=1:nbSamples
-	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6)
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[20,50,1300,500]);
diff --git a/demo_TPMFA01.m b/demo_TPMFA01.m
index b47018794d21fa61572ae14dfc5d8fc7dda82f46..29fb85d616306e11abee04ae28bf3c6a072a7772 100644
--- a/demo_TPMFA01.m
+++ b/demo_TPMFA01.m
@@ -1,9 +1,21 @@
 function demo_TPMFA01
-%Example of task-parameterized mixture of factor analyzers (TP-MFA)
-%Sylvain Calinon, 2015
+% Task-parameterized mixture of factor analyzers (TP-MFA)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...');
 % 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=2 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 (nbSamples=5).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/Data01.mat');
 
 
@@ -32,9 +44,10 @@ model = EM_tensorMFA(Data, model);
 
 %Reconstruct GMM for each demonstration
 for n=1:nbSamples
-	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6)
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[20,50,1300,500]);
diff --git a/demo_TPMPPCA01.m b/demo_TPMPPCA01.m
index 4a9071307554e9a61bd427c270743b1815444d73..847f3135e282e3e50d7b41624191129580be336e 100644
--- a/demo_TPMPPCA01.m
+++ b/demo_TPMPPCA01.m
@@ -1,9 +1,21 @@
 function demo_TPMPPCA01
-%Example of task-parameterized mixture of probabilistic principal component analyzers (MPPCA) (TP-GMM)
-%Sylvain Calinon, 2015
+% Task-parameterized mixture of probabilistic principal component analyzers (TP-MPPCA) 
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -19,8 +31,8 @@ disp('Load 3rd order tensor data...');
 % 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=2 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 (nbSamples=5).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/Data01.mat');
 
 
@@ -32,9 +44,10 @@ model = EM_tensorMPPCA(Data, model);
 
 %Reconstruct GMM for each demonstration
 for n=1:nbSamples
-	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p); %See Eq. (6)
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[20,50,1300,500]);
diff --git a/demo_TPbatchLQR01.m b/demo_TPbatchLQR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..0b3fd3225e30b8abf3f444f6e9a6c5beafb5f7b6
--- /dev/null
+++ b/demo_TPbatchLQR01.m
@@ -0,0 +1,230 @@
+function demo_TPbatchLQR01
+% Task-parameterized GMM encoding position and velocity data, combined with a batch solution 
+% of linear quadratic optimal control (unconstrained linear MPC)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
+model.rfactor = 1E-1;	%Control cost in LQR
+model.dt = 0.01; %Time step duration
+nbData = 200; %Number of datapoints in a trajectory
+nbRepros = 5; %Number of reproductions
+
+%Dynamical System settings (discrete version), see Eq. (33)
+A = kron([1, model.dt; 0, 1], eye(model.nbVarPos));
+B = kron([0; model.dt], eye(model.nbVarPos));
+%Control cost matrix
+R = eye(model.nbVarPos) * model.rfactor;
+R = kron(eye(nbData-1),R);
+
+
+%% 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 DC x P x N, with D=2 the dimension of a
+% datapoint, C=2 the number of derivatives (incl. position), P=2 the number of candidate frames, and N=TM 
+% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5).
+load('data/DataWithDeriv01.mat');
+
+% %Convert position data to position+velocity data
+% load('data/Data01.mat');
+% %Create transformation matrix to compute derivatives
+% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt;
+% D(end,end) = 0;
+% %Create 3rd order tensor data and task parameters
+% model.nbVar = model.nbVarPos * model.nbDeriv;
+% Data = zeros(model.nbVar, model.nbFrames, nbSamples*nbData);
+% for n=1:nbSamples
+% 	s(n).Data = zeros(model.nbVar,model.nbFrames,nbData);
+% 	s(n).Data0 = s(n).Data0(2:end,:); %Remove time
+% 	DataTmp = s(n).Data0;
+% 	for k=1:model.nbDeriv-1
+% 		DataTmp = [DataTmp; s(n).Data0*D^k]; %Compute derivatives
+% 	end
+% 	for m=1:model.nbFrames
+% 		s(n).p(m).b = [s(n).p(m).b; zeros((model.nbDeriv-1)*model.nbVarPos,1)];
+% 		s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A);
+% 		s(n).Data(:,m,:) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbData));
+% 		Data(:,m,(n-1)*nbData+1:n*nbData) = s(n).Data(:,m,:);
+% 	end
+% end
+% %Save new dataset including derivatives
+% save('data/DataWithDeriv01.mat', 'Data','s','nbSamples');
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-GMM with EM...');
+%model = init_tensorGMM_kmeans(Data, model); %Initialization
+
+%Initialization based on position data
+model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model);
+[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0);
+model.Priors = model0.Priors;
+for i=1:model.nbStates
+	for m=1:model.nbFrames
+		DataTmp = squeeze(Data(:,m,:));
+		model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)';
+		DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples);
+		model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
+	end
+end
+
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with batch LQR...');
+%Construct Su and Sx matrices, see Eq. (35)
+Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
+Sx = kron(ones(nbData,1),eye(model.nbVar)); 
+M = B;
+for n=2:nbData
+	%Build Sx matrix
+	id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
+	Sx(id1,:) = Sx(id1,:) * A;
+	%Build Su matrix
+	id1 = (n-1)*model.nbVar+1:n*model.nbVar; 
+	id2 = 1:(n-1)*model.nbVarPos;
+	Su(id1,id2) = M;
+	M = [A*M(:,1:model.nbVarPos), M];
+end
+
+%Reproductions
+for n=1:nbSamples
+	%Reconstruct GMM 
+	[s(n).Mu, s(n).Sigma] = productTPGMM0(model, s(n).p);
+	%Compute best path for the n-th demonstration
+	[~,s(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1	
+	%Build stepwise reference trajectory, see Eq. (27)
+	MuQ = reshape(s(n).Mu(:,s(n).q), model.nbVar*nbData, 1); 
+	SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(s(n).Sigma(:,:,s(n).q), model.nbVar, model.nbVar*nbData)) ...
+		.* kron(eye(nbData), ones(model.nbVar));
+	%Batch LQR (unconstrained linear MPC), see Eq. (37)
+	SuInvSigmaQ = Su' / SigmaQ;
+	Rq = SuInvSigmaQ * Su + R;
+	X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
+ 	rq = SuInvSigmaQ * (MuQ-Sx*X);
+ 	u = Rq \ rq; %can also be computed with u = lscov(Rq, rq);
+	r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with batch LQR...');
+for n=1:nbRepros
+	%Random generation of new task parameters
+	for m=1:model.nbFrames
+		id=ceil(rand(2,1)*nbSamples);
+		w=rand(2); w=w/sum(w);
+		rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+		rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+	end
+	%Reconstruct GMM 
+	[rnew(n).Mu, rnew(n).Sigma] = productTPGMM0(model, rnew(n).p);
+	%Compute best path for the 1st demonstration (HSMM can alternatively be used here)
+	[~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1	
+	%Build stepwise reference trajectory, see Eq. (27)
+	MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVar*nbData, 1); 
+	SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(rnew(n).Sigma(:,:,rnew(n).q), model.nbVar, model.nbVar*nbData)) ...
+		.* kron(eye(nbData), ones(model.nbVar));
+	%Batch LQR (unconstrained linear MPC), see Eq. (37)
+	SuInvSigmaQ = Su' / SigmaQ;
+	Rq = SuInvSigmaQ * Su + R;
+	X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
+ 	rq = SuInvSigmaQ * (MuQ-Sx*X);
+ 	u = Rq \ rq; %can also be computed with u = lscov(Rq, rq);
+	rnew(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(1,:), s(n).Data0(2,:),'-','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');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+end
+for n=1:nbSamples
+	%Plot trajectories
+	plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(r(n).Data(1,:), r(n).Data(2,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+for n=1:nbSamples
+	%Plot Gaussians
+	plotGMM(s(n).Mu(1:2,:,1), s(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions');
+for n=1:nbRepros
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+end
+for n=1:nbRepros
+	%Plot trajectories
+	plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]);
+	plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+for n=1:nbRepros
+	%Plot Gaussians
+	plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_TPbatchLQR01.png');
+%pause;
+%close all;
diff --git a/demo_TPbatchLQR02.m b/demo_TPbatchLQR02.m
new file mode 100644
index 0000000000000000000000000000000000000000..42a9176ca94b478bfbe711e61e38e1e8427be6bf
--- /dev/null
+++ b/demo_TPbatchLQR02.m
@@ -0,0 +1,232 @@
+function demo_TPbatchLQR02
+% Batch solution of a linear quadratic optimal control (unconstrained linear MPC) acting in multiple frames,
+% which is equivalent to TP-GMM combined with LQR
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
+model.rfactor = 1E-1;	%Control cost in LQR
+model.dt = 0.01; %Time step duration
+nbData = 200; %Number of datapoints in a trajectory
+nbRepros = 5; %Number of reproductions
+
+%Dynamical System settings (discrete version), see Eq. (33)
+A = kron([1, model.dt; 0, 1], eye(model.nbVarPos));
+B = kron([0; model.dt], eye(model.nbVarPos));
+%Control cost matrix
+R = eye(model.nbVarPos) * model.rfactor;
+R = kron(eye(nbData-1),R);
+
+
+%% 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 DC x P x N, with D=2 the dimension of a
+% datapoint, C=2 the number of derivatives (incl. position), P=2 the number of candidate frames, and N=TM 
+% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5).
+load('data/DataWithDeriv01.mat');
+
+
+%% TP-GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of TP-GMM with EM...');
+%model = init_tensorGMM_kmeans(Data, model); %Initialization
+
+%Initialization based on position data
+model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model);
+[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0);
+model.Priors = model0.Priors;
+for i=1:model.nbStates
+	for m=1:model.nbFrames
+		DataTmp = squeeze(Data(:,m,:));
+		model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)';
+		DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples);
+		model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
+	end
+end
+
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with batch LQR...');
+%Construct Su and Sx matrices, see Eq. (35)
+Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
+Sx = kron(ones(nbData,1),eye(model.nbVar)); 
+M = B;
+for n=2:nbData
+	%Build Sx matrix
+	id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
+	Sx(id1,:) = Sx(id1,:) * A;
+	%Build Su matrix
+	id1 = (n-1)*model.nbVar+1:n*model.nbVar; 
+	id2 = 1:(n-1)*model.nbVarPos;
+	Su(id1,id2) = M;
+	M = [A*M(:,1:model.nbVarPos), M];
+end
+
+%Reproductions
+for n=1:nbSamples
+	
+	%GMM projection, see Eq. (5)
+	for i=1:model.nbStates
+		for m=1:model.nbFrames
+			s(n).p(m).Mu(:,i) = s(n).p(m).A * model.Mu(:,m,i) + s(n).p(m).b;
+			s(n).p(m).Sigma(:,:,i) = s(n).p(m).A * model.Sigma(:,:,m,i) * s(n).p(m).A';
+		end
+	end
+	
+	%Compute best path for the n-th demonstration
+	[~,s(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1	
+	
+	%Build a reference trajectory for each frame, see Eq. (27)
+	invSigmaQ = zeros(model.nbVar*nbData);
+	for m=1:model.nbFrames
+		s(n).p(m).MuQ = reshape(s(n).p(m).Mu(:,s(n).q), model.nbVar*nbData, 1);  
+		s(n).p(m).SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(s(n).p(m).Sigma(:,:,s(n).q), model.nbVar, model.nbVar*nbData)) ...
+			.* kron(eye(nbData), ones(model.nbVar));
+		invSigmaQ = invSigmaQ + inv(s(n).p(m).SigmaQ);
+	end
+	
+	%Batch LQR (unconstrained linear MPC), see Eq. (37)
+	SuInvSigmaQ = Su' * invSigmaQ;
+	Rq = SuInvSigmaQ * Su + R;
+	X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
+ 	rq = zeros(model.nbVar*nbData,1);
+	for m=1:model.nbFrames
+		rq = rq + s(n).p(m).SigmaQ \ (s(n).p(m).MuQ - Sx*X);
+	end
+	rq = Su' * rq; 
+ 	u = Rq \ rq; %can also be computed with u = lscov(Rq, rq);
+	r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with batch LQR...');
+for n=1:nbRepros
+	
+	%Random generation of new task parameters
+	for m=1:model.nbFrames
+		id=ceil(rand(2,1)*nbSamples);
+		w=rand(2); w=w/sum(w);
+		rnew(n).p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+		rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+	end
+	
+	%GMM projection, see Eq. (5)
+	for i=1:model.nbStates
+		for m=1:model.nbFrames
+			rnew(n).p(m).Mu(:,i) = rnew(n).p(m).A * model.Mu(:,m,i) + rnew(n).p(m).b;
+			rnew(n).p(m).Sigma(:,:,i) = rnew(n).p(m).A * model.Sigma(:,:,m,i) * rnew(n).p(m).A';
+		end
+	end
+	
+	%Compute best path for the 1st demonstration (HSMM can alternatively be used here)
+	[~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1
+	
+	%Build a reference trajectory for each frame, see Eq. (27)
+	invSigmaQ = zeros(model.nbVar*nbData);
+	for m=1:model.nbFrames
+		rnew(n).p(m).MuQ = reshape(rnew(n).p(m).Mu(:,rnew(n).q), model.nbVar*nbData, 1);  
+		rnew(n).p(m).SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * ...
+			reshape(rnew(n).p(m).Sigma(:,:,rnew(n).q), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
+		invSigmaQ = invSigmaQ + inv(rnew(n).p(m).SigmaQ);
+	end
+	
+	%Batch LQR (unconstrained linear MPC), see Eq. (37)
+	SuInvSigmaQ = Su' * invSigmaQ;
+	Rq = SuInvSigmaQ * Su + R;
+	X = [s(1).Data0(:,1) + randn(model.nbVarPos,1)*0E0; zeros(model.nbVarPos,1)];
+ 	rq = zeros(model.nbVar*nbData,1);
+	for m=1:model.nbFrames
+		rq = rq + rnew(n).p(m).SigmaQ \ (rnew(n).p(m).MuQ - Sx*X);
+	end
+	rq = Su' * rq; 
+ 	u = Rq \ rq; %can also be computed with u = lscov(Rq, rq);
+	rnew(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+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(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+	%Plot trajectories
+	plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(1,:), s(n).Data0(2,:),'-','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');
+for n=1:nbSamples
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([s(n).p(m).b(1) s(n).p(m).b(1)+s(n).p(m).A(1,2)], [s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+end
+for n=1:nbSamples
+	%Plot trajectories
+	plot(r(n).Data(1,1), r(n).Data(2,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(r(n).Data(1,:), r(n).Data(2,:),'-','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');
+for n=1:nbRepros
+	%Plot frames
+	for m=1:model.nbFrames
+		plot([rnew(n).p(m).b(1) rnew(n).p(m).b(1)+rnew(n).p(m).A(1,2)], [rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,2)], '-','linewidth',6,'color',colPegs(m,:));
+		plot(rnew(n).p(m).b(1), rnew(n).p(m).b(2), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+	end
+end
+for n=1:nbRepros
+	%Plot trajectories
+	plot(rnew(n).Data(1,1), rnew(n).Data(2,1),'.','markersize',12,'color',[.2 .2 .2]);
+	plot(rnew(n).Data(1,:), rnew(n).Data(2,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%print('-dpng','graphs/demo_TPbatchLQR02.png');
+%pause;
+%close all;
diff --git a/demo_trajTPGMM01.m b/demo_TPtrajGMM01.m
similarity index 60%
rename from demo_trajTPGMM01.m
rename to demo_TPtrajGMM01.m
index 2ddf6e5e98a1ac6cbfecf2ef9a0ef6d2f4074289..81d49a47f5ea1d1d7f23f37d595a18883007c2e3 100644
--- a/demo_trajTPGMM01.m
+++ b/demo_TPtrajGMM01.m
@@ -1,17 +1,32 @@
-function demo_trajTPGMM01
-%Example of TP-GMM with trajectory GMM encoding
-%Sylvain Calinon, 2015
+function demo_TPtrajGMM01
+% Task-parameterized model with trajectory-GMM encoding (GMM with dynamic features)
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
 model.nbFrames = 2; %Number of candidate frames of reference
 model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
 model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
+model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
 model.dt = 0.01; %Time step
-nbRepros = 8; %Number of reproductions with new situations randomly generated
+nbData = 200; %Number of datapoints in a trajectory
+nbRepros = 5; %Number of reproductions with new situations randomly generated
+
 
 %% Load 3rd order tensor data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -19,47 +34,64 @@ 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
-% multiplied by the number of demonstrations.
-load('data/DataLQR01.mat');
-
-%Compute derivatives
-nbD = s(1).nbData;
-%Create transformation matrix to compute [X; DX; DDX]
-D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
-D(end,end) = 0;
-%Create 3rd order tensor data with XHAT instead of X
-model.nbVar = model.nbVarPos * model.nbDeriv;
-Data = zeros(model.nbVar, model.nbFrames, nbD);
-for n=1:nbSamples
-	DataTmp = s(n).Data0(2:end,:);
-	for k=1:model.nbDeriv-1
-		DataTmp = [DataTmp; s(n).Data0(2:end,:)*D^k]; %Compute derivatives
-	end
-	for m=1:model.nbFrames
-		s(n).p(m).b = [s(n).p(m).b(2:end); zeros((model.nbDeriv-1)*model.nbVarPos,1)];
-		s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A(2:end,2:end));
-		Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD));
-	end
-end
+% in the different frames. It is a 3rd order tensor of dimension DC x P x N, with D=2 the dimension of a
+% datapoint, C=2 the number of derivatives (incl. position), P=2 the number of candidate frames, and N=TM 
+% the number of datapoints in a trajectory (T=200) multiplied by the number of demonstrations (M=5).
+load('data/DataWithDeriv02.mat');
+
+% %Convert position data to position+velocity data
+% load('data/Data01.mat');
+% %Create transformation matrix to compute derivatives
+% D = (diag(ones(1,nbData-1),-1)-eye(nbData)) / model.dt;
+% D(end,end) = 0;
+% %Create 3rd order tensor data and task parameters
+% Data = zeros(model.nbVar, model.nbFrames, nbSamples*nbData);
+% for n=1:nbSamples
+% 	s(n).Data = zeros(model.nbVar,model.nbFrames,nbData);
+% 	s(n).Data0 = s(n).Data0(2:end,:); %Remove time
+% 	DataTmp = s(n).Data0;
+% 	for k=1:model.nbDeriv-1
+% 		DataTmp = [DataTmp; s(n).Data0*D^k]; %Compute derivatives
+% 	end
+% 	for m=1:model.nbFrames
+% 		s(n).p(m).b = [s(n).p(m).b; zeros((model.nbDeriv-1)*model.nbVarPos,1)];
+% 		s(n).p(m).A = kron(eye(model.nbDeriv), s(n).p(m).A);
+% 		s(n).Data(:,m,:) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbData));
+% 		Data(:,m,(n-1)*nbData+1:n*nbData) = s(n).Data(:,m,:);
+% 	end
+% end
+% %Save new dataset including derivatives
+% save('data/DataWithDeriv02.mat', 'Data','s','nbSamples');
 
 %Construct PHI operator (big sparse matrix)
-[PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples); 
+[PHI,PHI1] = constructPHI(model, nbData, nbSamples); 
 
 
 %% TP-GMM learning
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-fprintf('Parameters estimation of TP-GMM with EM:');
+fprintf('Parameters estimation of TP-GMM with EM...');
 %model = init_tensorGMM_timeBased(Data, model); %Initialization
-model = init_tensorGMM_kmeans(Data, model); %Initialization
+%model = init_tensorGMM_kmeans(Data, model); %Initialization
+
+%Initialization based on position data
+model0 = init_tensorGMM_kmeans(Data(1:model.nbVarPos,:,:), model);
+[~,~,GAMMA2] = EM_tensorGMM(Data(1:model.nbVarPos,:,:), model0);
+model.Priors = model0.Priors;
+for i=1:model.nbStates
+	for m=1:model.nbFrames
+		DataTmp = squeeze(Data(:,m,:));
+		model.Mu(:,m,i) = DataTmp * GAMMA2(i,:)';
+		DataTmp = DataTmp - repmat(model.Mu(:,m,i),1,nbData*nbSamples);
+		model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
+	end
+end
+
 model = EM_tensorGMM(Data, model);
 
 
 %% Reproduction for the task parameters used to train the model
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 disp('Reproductions...');
-%DataIn = [1:s(1).nbData] * model.dt;
 for n=1:nbSamples
 	%Products of linearly transformed Gaussians
 	for i=1:model.nbStates
@@ -74,19 +106,19 @@ for n=1:nbSamples
 		r(n).Sigma(:,:,i) = inv(SigmaTmp);
 		r(n).Mu(:,i) = r(n).Sigma(:,:,i) * MuTmp;
 	end
-	%Create single Gaussian N(MuQ,SigmaQ) based on state sequence q
-	[~,r(n).q] = max(model.Pix(:,(n-1)*T1+1:n*T1),[],1); %works also for nbStates=1
-	r(n).MuQ = reshape(r(n).Mu(:,r(n).q), model.nbVarPos*model.nbDeriv*T1, 1);
-	r(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1);
-	for t=1:T1
+	%Create single Gaussian N(MuQ,SigmaQ) based on state sequence q, see Eq. (27)
+	[~,r(n).q] = max(model.Pix(:,(n-1)*nbData+1:n*nbData),[],1); %works also for nbStates=1
+	r(n).MuQ = reshape(r(n).Mu(:,r(n).q), model.nbVarPos*model.nbDeriv*nbData, 1);
+	r(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*nbData);
+	for t=1:nbData
 		id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv;
 		r(n).SigmaQ(id1,id1) = r(n).Sigma(:,:,r(n).q(t));
 	end
-	%Retrieval of data with trajectory GMM
+	%Retrieval of data with trajectory GMM, see Eq. (30)
 	PHIinvSigmaQ = PHI1'/r(n).SigmaQ;
 	Rq = PHIinvSigmaQ * PHI1;
 	rq = PHIinvSigmaQ * r(n).MuQ;
-	r(n).Data = reshape(Rq\rq, model.nbVarPos, T1); %Reshape data for plotting
+	r(n).Data = reshape(Rq\rq, model.nbVarPos, nbData); %Reshape data for plotting
 end
 
 
@@ -114,19 +146,19 @@ for n=1:nbRepros
 		rnew(n).Sigma(:,:,i) = inv(SigmaTmp);
 		rnew(n).Mu(:,i) = rnew(n).Sigma(:,:,i) * MuTmp;
 	end
-	%Create single Gaussian N(MuQ,SigmaQ) based on state sequence q
-	[~,rnew(n).q] = max(model.Pix(:,1:T1),[],1); %works also for nbStates=1
-	rnew(n).MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVarPos*model.nbDeriv*T1, 1);
-	rnew(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*T1);
-	for t=1:T1
+	%Create single Gaussian N(MuQ,SigmaQ) based on state sequence q, see Eq. (27)
+	[~,rnew(n).q] = max(model.Pix(:,1:nbData),[],1); %works also for nbStates=1
+	rnew(n).MuQ = reshape(rnew(n).Mu(:,rnew(n).q), model.nbVarPos*model.nbDeriv*nbData, 1);
+	rnew(n).SigmaQ = zeros(model.nbVarPos*model.nbDeriv*nbData);
+	for t=1:nbData
 		id1 = (t-1)*model.nbVarPos*model.nbDeriv+1:t*model.nbVarPos*model.nbDeriv;
 		rnew(n).SigmaQ(id1,id1) = rnew(n).Sigma(:,:,rnew(n).q(t));
 	end
-	%Retrieval of data with trajectory GMM
+	%Retrieval of data with trajectory GMM, see Eq. (30)
 	PHIinvSigmaQ = PHI1'/rnew(n).SigmaQ;
 	Rq = PHIinvSigmaQ * PHI1;
 	rq = PHIinvSigmaQ * rnew(n).MuQ;
-	rnew(n).Data = reshape(Rq\rq, model.nbVarPos, T1); %Reshape data for plotting
+	rnew(n).Data = reshape(Rq\rq, model.nbVarPos, nbData); %Reshape data for plotting
 end
 
 
@@ -148,8 +180,8 @@ for n=1:nbSamples
 		plot(s(n).p(m).b(1), s(n).p(m).b(2),'.','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,:));
+	plot(s(n).Data0(1,1), s(n).Data0(2,1),'.','markersize',12,'color',clrmap(n,:));
+	plot(s(n).Data0(1,:), s(n).Data0(2,:),'-','linewidth',1.5,'color',clrmap(n,:));
 end
 axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
@@ -169,7 +201,7 @@ for n=1:nbSamples
 end
 for n=1:nbSamples
 	%Plot Gaussians
-	plotGMM(r(n).Mu(1:2,:,1), r(n).Sigma(1:2,1:2,:,1), [.5 .5 .5],.8);
+	plotGMM(r(n).Mu(1:2,:,1), r(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4);
 end
 axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
@@ -189,11 +221,11 @@ for n=1:nbRepros
 end
 for n=1:nbRepros
 	%Plot Gaussians
-	plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5],.8);
+	plotGMM(rnew(n).Mu(1:2,:,1), rnew(n).Sigma(1:2,1:2,:,1), [.5 .5 .5], .4);
 end
 axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
 
-%print('-dpng','graphs/demo_trajTPGMM01.png');
+%print('-dpng','graphs/demo_TPtrajGMM01.png');
 %pause;
 %close all;
 
diff --git a/demo_batchLQR01.m b/demo_batchLQR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..4ce8d4c15980591d8a04a5e95ef623c3fb75be94
--- /dev/null
+++ b/demo_batchLQR01.m
@@ -0,0 +1,147 @@
+function demo_batchLQR01
+% Controller retrieval through a batch solution of linear quadratic optimal control (unconstrained linear MPC),
+% by relying on a Gaussian mixture model (GMM) encoding of position and velocity data.
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbSamples = 3; %Number of demonstrations
+nbRepros = 5; %Number of reproductions
+nbData = 100; %Number of datapoints
+
+model.nbStates = 5; %Number of Gaussians in the GMM
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
+model.rfactor = 1E-6;	%Control cost in LQR
+model.dt = 0.01; %Time step duration
+
+%Dynamical System settings (discrete version), see Eq. (33)
+A = kron([1, model.dt; 0, 1], eye(model.nbVarPos));
+B = kron([0; model.dt], eye(model.nbVarPos));
+C = kron([1, 0], eye(model.nbVarPos));
+%Control cost matrix
+R = eye(model.nbVarPos) * model.rfactor;
+R = kron(eye(nbData-1),R);
+
+
+%% Load AMARSI handwriting data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+load('data/AMARSI/GShape.mat')
+Data=[];
+for n=1:nbSamples
+	s(n).Data=[];
+	for m=1:model.nbDeriv
+		if m==1
+			dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+		else
+			dTmp = gradient(dTmp) / model.dt; %Compute derivatives
+		end
+		s(n).Data = [s(n).Data; dTmp];
+	end
+	Data = [Data s(n).Data]; 
+end
+
+
+%% Learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%model = init_GMM_kmeans(Data,model);
+
+%Initialization based on position data
+model0 = init_GMM_kmeans(Data(1:model.nbVarPos,:), model);
+[~, GAMMA2] = EM_GMM(Data(1:model.nbVarPos,:), model0);
+model.Priors = model0.Priors;
+for i=1:model.nbStates
+	model.Mu(:,i) = Data * GAMMA2(i,:)';
+	DataTmp = Data - repmat(model.Mu(:,i),1,nbData*nbSamples);
+	model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
+end
+
+%Refinement of parameters
+[model, H] = EM_GMM(Data, model);
+
+%Set list of states according to first demonstration (alternatively, an HSMM can be used)
+[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1
+
+%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (27)
+MuQ = reshape(model.Mu(:,qList), model.nbVar*nbData, 1); 
+SigmaQ = zeros(model.nbVar*nbData);
+for t=1:nbData
+	id = (t-1)*model.nbVar+1:t*model.nbVar;
+	%MuQ(id,1) = model.Mu(:,qList(t));
+	SigmaQ(id,id) = model.Sigma(:,:,qList(t));
+end	
+%SigmaQ can alternatively be computed with: 
+%SigmaQ = (kron(ones(nbData,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,qList), model.nbVar, model.nbVar*nbData)) .* kron(eye(nbData), ones(model.nbVar));
+
+
+%% Batch LQR reproduction
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+Su = zeros(model.nbVar*nbData, model.nbVarPos*(nbData-1));
+Sx = kron(ones(nbData,1),eye(model.nbVar)); 
+M = B;
+for n=2:nbData
+	%Build Sx matrix, see Eq. (35)
+	id1 = (n-1)*model.nbVar+1:nbData*model.nbVar;
+	Sx(id1,:) = Sx(id1,:) * A;
+	%Build Su matrix, see Eq. (35)
+	id1 = (n-1)*model.nbVar+1:n*model.nbVar; 
+	id2 = 1:(n-1)*model.nbVarPos;
+	Su(id1,id2) = M;
+	M = [A*M(:,1:model.nbVarPos), M];
+end
+
+%Set matrices to compute the damped weighted least squares estimate, see Eq. (37)
+SuInvSigmaQ = Su' / SigmaQ;
+Rq = SuInvSigmaQ * Su + R;
+%M = (SuInvSigmaQ * Su + R) \ SuInvSigmaQ; 
+%M = inv(Su' / SigmaQ * Su + R) * Su' /SigmaQ;
+
+%Reproductions
+for n=1:nbRepros
+	X = Data(:,1) + [randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)];
+ 	rq = SuInvSigmaQ * (MuQ-Sx*X);
+ 	u = Rq \ rq; %Can also be computed with u = lscov(Rq, rq);
+	%u = M * (MuQ-Sx*X);
+	r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+	
+% 	%Simulate plant
+% 	r(n).u = reshape(u, model.nbVarPos, nbData-1);
+% 	for t=1:nbData-1
+% 		%id = (t-1)*model.nbVar+1:t*model.nbVar;
+% 		%r(n).u(:,t) = M(id,id) * (MuQ(id) - X(1:model.nbVar));
+% 		X = A * X + B * r(n).u(:,t);
+% 		r(n).Data(:,t) = X;
+% 	end
+end
+
+
+%% Plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
+plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5]);
+plot(Data(1,:), Data(2,:), 'k.');
+for n=1:nbRepros
+	plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
+	plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',16,'color',[1 0 0]);
+end
+axis equal; 
+
+%print('-dpng','graphs/demo_batchLQR01.png');
+%pause;
+%close all;
+
diff --git a/demo_iterativeLQR01.m b/demo_iterativeLQR01.m
new file mode 100644
index 0000000000000000000000000000000000000000..b3db7f440d1894d53ca4b26b01a364d7ad20d1c7
--- /dev/null
+++ b/demo_iterativeLQR01.m
@@ -0,0 +1,115 @@
+function demo_iterativeLQR01
+% Example of controller retrieval through an iterative solution of linear quadratic optimal control (finite horizon, 
+% unconstrained linear MPC), by relying on a Gaussian mixture model (GMM) encoding of position and velocity data
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
+
+addpath('./m_fcts/');
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+nbSamples = 3; %Number of demonstrations
+nbRepros = 5; %Number of reproductions
+nbData = 100; %Number of datapoints
+
+model.nbStates = 5; %Number of Gaussians in the GMM
+model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
+model.nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
+model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
+model.rfactor = 1E-6;	%Control cost in LQR
+model.dt = 0.01; %Time step duration
+
+%Dynamical System settings (discrete version)
+A = kron([1, model.dt; 0, 1], eye(model.nbVarPos));
+B = kron([0; model.dt], eye(model.nbVarPos));
+C = kron([1, 0], eye(model.nbVarPos));
+%Control cost matrix
+R = eye(model.nbVarPos) * model.rfactor;
+
+
+%% Load Data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+load('data/AMARSI/GShape.mat')
+Data=[];
+for n=1:nbSamples
+	s(n).Data=[];
+	for m=1:model.nbDeriv
+		if m==1
+			dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
+		else
+			dTmp = gradient(dTmp) / model.dt; %Compute derivatives
+		end
+		s(n).Data = [s(n).Data; dTmp];
+	end
+	Data = [Data s(n).Data]; 
+end
+
+
+%% Learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%model = init_GMM_kmeans(Data,model);
+
+%Initialization based on position data
+model0 = init_GMM_kmeans(Data(1:model.nbVarPos,:), model);
+[~, GAMMA2] = EM_GMM(Data(1:model.nbVarPos,:), model0);
+model.Priors = model0.Priors;
+for i=1:model.nbStates
+	model.Mu(:,i) = Data * GAMMA2(i,:)';
+	DataTmp = Data - repmat(model.Mu(:,i),1,nbData*nbSamples);
+	model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp';
+end
+
+%Refinement of parameters
+[model, H] = EM_GMM(Data, model);
+
+%Set list of states according to first demonstration (alternatively, an HSMM can be used)
+[~,qList] = max(H(:,1:nbData),[],1); %works also for nbStates=1
+
+
+%% Iterative LQR reproduction (finite horizon)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+P = zeros(model.nbVar,model.nbVar,nbData);
+P(:,:,end) = inv(model.Sigma(:,:,qList(nbData)));
+for t=nbData-1:-1:1
+	Q = inv(model.Sigma(:,:,qList(t)));
+	P(:,:,t) = Q - A' * (P(:,:,t+1) * B / (B' * P(:,:,t+1) * B + R) * B' * P(:,:,t+1) - P(:,:,t+1)) * A;
+end
+%Reproduction
+for n=1:nbRepros
+	x = Data(1:model.nbVarPos,1) + randn(model.nbVarPos,1)*2E0;
+	dx = Data(model.nbVarPos+1:end,1);
+	for t=1:nbData
+		K = (B' * P(:,:,t) * B + R) \ B' * P(:,:,t) * A;
+		ddx = K * (model.Mu(:,qList(t)) - [x; dx]);
+		dx = dx + ddx * model.dt;
+		x = x + dx * model.dt;
+		r(n).Data(:,t) = [x;dx;ddx];
+	end
+end
+
+
+%% Plot
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[10 10 700 700],'color',[1 1 1]); hold on; axis off;
+plotGMM(model.Mu(1:2,:), model.Sigma(1:2,1:2,:), [0.5 0.5 0.5]);
+plot(Data(1,:), Data(2,:), 'k.');
+for n=1:nbRepros
+	plot(r(n).Data(1,:), r(n).Data(2,:), '-','linewidth',2,'color',[.8 0 0]);
+	plot(r(n).Data(1,:), r(n).Data(2,:), '.','markersize',16,'color',[1 0 0]);
+end
+axis equal; 
+
+%print('-dpng','graphs/demo_iterativeLQR01.png');
+pause;
+close all;
+
diff --git a/demo_stdPGMM01.m b/demo_stdPGMM01.m
index 675377300dc9e175c91aa770e2b5e4e91e732f90..3faf987c5bc0056c89dc43b10da66c53ccb71c10 100644
--- a/demo_stdPGMM01.m
+++ b/demo_stdPGMM01.m
@@ -1,10 +1,22 @@
 function demo_stdPGMM01
-%Example of parametric Gaussian mixture model (PGMM) used as a task-parameterized model,
-%with DS-GMR employed to retrieve continuous movements
-%Sylvain Calinon, 2015
+% Example of parametric Gaussian mixture model (PGMM) used for task adaptation,
+% with DS-GMR employed to retrieve continuous movements
+%
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
+%
+% 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:
+%
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
+% }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbStates = 3; %Number of Gaussians in the GMM
@@ -23,8 +35,8 @@ disp('Load motion data...');
 % 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 (nbSamples=5).
+% datapoint, P=2 the number of candidate frames, and N=TM the number of datapoints in a trajectory (T=200)
+% multiplied by the number of demonstrations (M=5).
 load('data/DataLQR01.mat');
 
 
@@ -38,7 +50,7 @@ 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 data with XHAT instead of X, see Eq. (4.0.2) in doc/TechnicalReport.pdf
+%Compute derivatives
 Data = [];
 for n=1:nbSamples
 	DataTmp = s(n).Data0(2:end,:);
@@ -51,9 +63,10 @@ end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 fprintf('Parameters estimation of PGMM with EM:');
 for n=1:nbSamples	
-	%Task parameters rearranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+	%Task parameters rearranged as a vector (position and orientation), see Eq. (48)
 	s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(1).A(2:3,3); s(n).p(2).b(2:3); s(n).p(2).A(2:3,3); 1];
-% 	%Task parameters rearranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+	
+% 	%Task parameters rearranged as a vector (position only), see Eq. (48)
 % 	s(n).OmegaMu = [s(n).p(1).b(2:3); s(n).p(2).b(2:3); 1];
 end
 
@@ -64,6 +77,7 @@ for i=1:model.nbStates
 	%Initialization of parameters based on standard GMM
 	model.ZMu(:,:,i) = zeros(model.nbVar, size(s(1).OmegaMu,1));
 	model.ZMu(:,end,i) = model.Mu(:,i);
+	
 	% 	%Random initialization of parameters
 	% 	model.ZMu(:,:,i) = rand(model.nbVar,size(s(1).OmegaMu,1));
 	% 	model.Sigma(:,:,i) = eye(model.nbVar);
@@ -83,17 +97,17 @@ L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
 for n=1:nbSamples
 	%Computation of the resulting Gaussians (for display purpose)
 	for i=1:model.nbStates
-		model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf
+		model.Mu(:,i) = model.ZMu(:,:,i) * s(n).OmegaMu; %Temporary Mu variable, see Eq. (48)
 	end
 	r(n).Mu = model.Mu;
-	%Retrieval of attractor path through GMR
-	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	%Retrieval of attractor path through GMR, see Eq. (17)-(19)
+	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); 
 	%Motion retrieval with spring-damper system
 	x = s(n).p(1).b(2:model.nbVar);
 	dx = zeros(nbVarOut,1);
 	for t=1:s(n).nbData
 		%Compute acceleration, velocity and position
-		ddx =  -L * [x-currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
+		ddx =  -L * [x-currTar(:,t); dx]; 
 		dx = dx + ddx * model.dt;
 		x = x + dx * model.dt;
 		r(n).Data(:,t) = x;
@@ -113,18 +127,19 @@ for n=1:nbRepros
 		rnew(n).p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
 	end
 	
-	%Task parameters re-arranged as a vector (position and orientation), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+	%Task parameters re-arranged as a vector (position and orientation), see Eq. (48) 
 	rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(1).A(2:3,3); rnew(n).p(2).b(2:3); rnew(n).p(2).A(2:3,3); 1];
-% 	%Task parameters re-arranged as a vector (position only), see Eq. (7.1.4) in doc/TechnicalReport.pdf
+	
+% 	%Task parameters re-arranged as a vector (position only), see Eq. (48) 
 % 	rnew(n).OmegaMu = [rnew(n).p(1).b(2:3); rnew(n).p(2).b(2:3); 1];
 	
 	%Computation of the resulting Gaussians (for display purpose)
 	for i=1:model.nbStates
-		model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable, see Eq. (7.1.4) in doc/TechnicalReport.pdf
+		model.Mu(:,i) = model.ZMu(:,:,i) * rnew(n).OmegaMu; %Temporary Mu variable, see Eq. (48)
 	end
 	rnew(n).Mu = model.Mu;
-	%Retrieval of attractor path through GMR
-	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
+	%Retrieval of attractor path through GMR, see Eq. (17)-(19)
+	currTar = GMR(model, DataIn, 1, [2:model.nbVar]); 
 	%Motion retrieval with spring-damper system
 	x = rnew(n).p(1).b(2:model.nbVar);
 	dx = zeros(nbVarOut,1);
diff --git a/demo_testLQR01.m b/demo_testLQR01.m
index 08e0e0750d42e4e6203b7193ad5613903facb8e8..5562cf2762ca754e0419b6c4b2251a4c2ed2b2aa 100644
--- a/demo_testLQR01.m
+++ b/demo_testLQR01.m
@@ -1,24 +1,21 @@
 function demo_testLQR01
 % Test of the linear quadratic regulation with different variance in the data
 %
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
 %
 % 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"
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
 % }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
@@ -27,6 +24,7 @@ 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...');
@@ -34,10 +32,12 @@ 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);
+	%r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor);
+	r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor);
+	r(n).Data = [DataIn; r(n).Data];
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[20,50,1300,500]);
@@ -64,6 +64,7 @@ for n=1:nbRepros
 end
 xlabel('t'); ylabel('kp');
 
+%print('-dpng','graphs/demo_testLQR01.png');
 %pause;
 %close all;
 
diff --git a/demo_testLQR02.m b/demo_testLQR02.m
index 787a1d7d72bf6cb8c22169dc4fee50a2a62407d7..93bb4982d1366eb97b3d3fd65352bfb8ee4019eb 100644
--- a/demo_testLQR02.m
+++ b/demo_testLQR02.m
@@ -1,24 +1,21 @@
 function demo_testLQR02
 % Test of the linear quadratic regulation (evaluation of the damping ratio found by the system)
 %
-% Author:	Sylvain Calinon, 2014
-%         http://programming-by-demonstration.org/SylvainCalinon
+% Sylvain Calinon, 2015
+% http://programming-by-demonstration.org/lib/
 %
 % 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"
+% @article{Calinon15,
+%   author="Calinon, S.",
+%   title="A tutorial on task-parameterized movement learning and retrieval",
+%   year="2015",
 % }
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
@@ -27,6 +24,7 @@ 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...');
@@ -34,10 +32,12 @@ DataIn = [1:nbData] * model.dt;
 a.currTar = ones(1,nbData);
 a.currSigma = ones(1,1,nbData); %-> 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);
+	%r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor);
+	r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor);
+	r(n).Data = [DataIn; r(n).Data];
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[20,50,1300,500]);
@@ -72,6 +72,7 @@ for n=1:nbRepros
 end
 xlabel('t'); ylabel('Damping ratio');
 
+%print('-dpng','graphs/demo_testLQR02.png');
 %pause;
 %close all;
 
diff --git a/demo_testLQR03.m b/demo_testLQR03.m
index 294ea3183bf2e7f11f7f38f37625ec1fdd891816..165e4567d1f52aef937c1992df0538e28362d47c 100644
--- a/demo_testLQR03.m
+++ b/demo_testLQR03.m
@@ -19,6 +19,7 @@ function demo_testLQR03
 
 addpath('./m_fcts/');
 
+
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
@@ -27,6 +28,7 @@ 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...');
@@ -43,12 +45,14 @@ aFinal.currSigma = a.currSigma(:,:,end);
 
 for n=1:nbRepros
 	if n==1
-		r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+		r(n) = reproduction_LQR_infiniteHorizon(model, a, 0, rFactor);
+		r(n).Data = [DataIn; r(n).Data];
 	else
 		%First call to LQR to get an estimate of the final feedback terms
-		[~,Pfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
+		[~,Pfinal] = reproduction_LQR_infiniteHorizon(model, aFinal, 0, rFactor);
 		%Second call to LQR with finite horizon
-		r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Pfinal);
+		r(n) = reproduction_LQR_finiteHorizon(model, a, 0, rFactor, Pfinal);
+		r(n).Data = [DataIn; r(n).Data];
 	end
 end
 for n=1:nbRepros
@@ -58,6 +62,7 @@ for n=1:nbRepros
 	end
 end
 
+
 %% Plots
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[20,50,1300,500]);
@@ -96,5 +101,6 @@ for n=1:nbRepros
 end
 xlabel('t'); ylabel('kv');
 
+%print('-dpng','graphs/demo_testLQR03.png');
 %pause;
 %close all;
diff --git a/demo_trajGMM01.m b/demo_trajGMM01.m
deleted file mode 100644
index 4ec4daaddbe2dc446133c080a31d08a743424c2f..0000000000000000000000000000000000000000
--- a/demo_trajGMM01.m
+++ /dev/null
@@ -1,189 +0,0 @@
-function demo_trajGMM01
-%Example of trajectory synthesis with a GMM with dynamic features (trajectory GMM)
-%Sylvain Calinon, 2015
-
-addpath('./m_fcts/');
-
-%% Parameters
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbStates = 5; %Number of components in the GMM
-model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
-model.nbDeriv = 3; %Number of static&dynamic features (D=2 for [x,dx], D=3 for [x,dx,ddx], etc.)
-model.nbVar = model.nbVarPos * model.nbDeriv;
-model.dt = 1; %Time step (without rescaling, large values such as 1 has the advantage of creating clusers based on position information)
-nbSamples = 4; %Number of demonstrations
-nbD = 200; %Number of datapoints in a trajectory
-
-[PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples); %Construct PHI operator (big sparse matrix)
-
-
-%% Load dataset
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-load('data/dataTrajGMM01.mat');
-%load('data/dataTrajGMM02.mat'); %periodic motion
-%load('data/dataTrajGMM03.mat'); %motion with options
-%load('data/dataTrajGMM04.mat'); %partial demonstrations
-
-%Resampling of dataset
-nbD0 = size(Data,2)/nbSamples; %Original number of datapoints in a trajectory
-DataTmp = [];
-for n=1:nbSamples
-	DataTmp = [DataTmp spline(1:nbD0, Data(2:model.nbVarPos+1,(n-1)*nbD0+1:n*nbD0), linspace(1,nbD0,nbD))]; 
-end
-Data = DataTmp;
-
-%Re-arrange data in vector form
-x = reshape(Data(:,1:T), model.nbVarPos*T, 1) * 1E2; %Scale data to avoid numerical computation problem
-zeta = PHI*x; %y is for example [x1(1), x2(1), x1d(1), x2d(1), x1(2), x2(2), x1d(2), x2d(2), ...], see Eq. (2.4.5) in doc/TechnicalReport.pdf
-Data = reshape(zeta, model.nbVarPos*model.nbDeriv, T); %Include derivatives in Data
-
-
-%% Parameters estimation
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Initialization with kmeans
-model = init_GMM_kmeans(Data, model);
-
-% %Initialization with homogeneous time intervals
-% DataTS = [repmat([1:T1]*model.dt,1,nbSamples); Data];
-% modelTmp = init_GMM_timeBased(DataTS, model);
-% model.Mu = modelTmp.Mu(2:end,:);
-% model.Sigma = modelTmp.Sigma(2:end,2:end,:);
-% model.Priors = modelTmp.Priors;
-
-%Refinement of parameters
-[model, GAMMA2] = EM_GMM(Data, model);
-%[model, GAMMA2] = EM_MFA(Data, model);
-%[model, GAMMA2] = EM_MPPCA(Data, model);
-%[model, GAMMA2] = EM_HDGMM(Data, model);
-
-%Precomputation of inverses (optional)
-for i=1:model.nbStates
-	model.invSigma(:,:,i) = inv(model.Sigma(:,:,i));
-end
-
-
-%% Reproduction
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-for n=1:1 %nbSamples
-	%Compute best path for the n-th demonstration
-	[~,r(n).q] = max(GAMMA2(:,(n-1)*T1+1:n*T1),[],1); %works also for nbStates=1
-	
-	%Create single Gaussian N(MuQ,SigmaQ) based on optimal state sequence q, see Eq. (2.4.8) in doc/TechnicalReport.pdf
-	MuQ = reshape(model.Mu(:,r(n).q), model.nbVar*T1, 1); 
-	%MuQ = zeros(model.nbVar*T1,1);
-	SigmaQ = zeros(model.nbVar*T1);
-	for t=1:T1
-		id = (t-1)*model.nbVar+1:t*model.nbVar;
-		%MuQ(id) = model.Mu(:,r(n).q(t)); 
-		SigmaQ(id,id) = model.Sigma(:,:,r(n).q(t));
-	end
-	%SigmaQ can alternatively be computed with:
-	%r(n).SigmaQ = (kron(ones(T1,1), eye(model.nbVar)) * reshape(model.Sigma(:,:,r(1).q), model.nbVar, model.nbVar*T1)) .* kron(eye(T1), ones(model.nbVar));
-
-
-	%Least squares computation method 1 (using lscov Matlab function), see Eq. (2.4.11) in doc/TechnicalReport.pdf
-	%%%%%%%%%%%%%%%%%%%
-	[zeta,~,mse,S] = lscov(PHI1, MuQ, SigmaQ,'chol'); %Retrieval of data with weighted least squares solution
-	r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting
-
-
-% 	%Least squares computation method 2 (most readable but not optimized), see Eq. (2.4.11) in doc/TechnicalReport.pdf
-% 	%%%%%%%%%%%%%%%%%%%
-% 	PHIinvSigmaQ = PHI1' / SigmaQ;
-% 	Rq = PHIinvSigmaQ * PHI1;
-% 	rq = PHIinvSigmaQ * MuQ;
-% 	zeta = Rq \ rq; %Can also be computed with c = lscov(Rq, rq)
-% 	r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting
-% 	%Covariance Matrix computation of ordinary least squares estimate, see Eq. (2.4.12) in doc/TechnicalReport.pdf
-% 	mse =  (MuQ'*inv(SigmaQ)*MuQ - rq'*inv(Rq)*rq)  ./ ((model.nbVar-model.nbVarPos)*T1);
-% 	S = inv(Rq) * mse; 
-
-
-% 	%Least squares computation method 3 (efficient computation using Cholesky and QR decompositions, inspired by lscov code)
-% 	%%%%%%%%%%%%%%%%%%%
-% 	T = chol(SigmaQ)'; %SigmaQ=T*T'
-% 	TA = T \ PHI1;
-% 	TMuQ = T \ MuQ;
-% 	[Q, R, perm] = qr(TA,0); %PHI1(:,perm)=Q*R
-% 	z = Q' * TMuQ;
-% 	zeta(perm,:) = R \ z; %zeta=(TA'*TA)\(TA'*TMuQ), see Eq. (2.4.11) in doc/TechnicalReport.pdf
-% 	r(n).Data = reshape(zeta, model.nbVarPos, T1); %Reshape data for plotting
-% 	%Covariance Matrix computation of ordinary least squares estimate
-% 	err = TMuQ - Q*z;
-% 	mse = err'*err ./ (model.nbVar*T1 - model.nbVarPos*T1);
-% 	Rinv = R \ eye(model.nbVarPos*T1);
-% 	S(perm,perm) = Rinv*Rinv' .* mse; %See Eq. (2.4.12) in doc/TechnicalReport.pdf
-	
-	
-	%Rebuild covariance by reshaping S, see Eq. (2.4.12) in doc/TechnicalReport.pdf
-	for t=1:T1
-		id = (t-1)*model.nbVarPos+1:t*model.nbVarPos;
-		r(n).expSigma(:,:,t) = S(id,id) * T1;
-	end
-	
-end %nbSamples
-
-
-%% Plot timeline
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 600 600]);
-for m=1:model.nbVarPos
-	limAxes = [1, T1, min(Data(m,:))-1E0, max(Data(m,:))+1E0];
-	subplot(model.nbVarPos,1,m); hold on;
-	for n=1:1 %nbSamples
-		msh=[]; x0=[];
-		for t=1:T1-1
-			if size(msh,2)==0
-				msh(:,1) = [t; model.Mu(m,r(n).q(t))];
-			end
-			if t==T1-1 || r(n).q(t+1)~=r(n).q(t)
-				msh(:,2) = [t+1; model.Mu(m,r(n).q(t))];
-				sTmp = model.Sigma(m,m,r(n).q(t))^.5;
-				msh2 = [msh(:,1)+[0;sTmp], msh(:,2)+[0;sTmp], msh(:,2)-[0;sTmp], msh(:,1)-[0;sTmp], msh(:,1)+[0;sTmp]];
-				patch(msh2(1,:), msh2(2,:), [.85 .85 .85],'edgecolor',[.7 .7 .7]);
-				plot(msh(1,:), msh(2,:), '-','linewidth',3,'color',[.7 .7 .7]);
-				plot([msh(1,1) msh(1,1)], limAxes(3:4), ':','linewidth',1,'color',[.7 .7 .7]);
-				x0 = [x0 msh];
-				msh=[];
-			end
-		end
-		msh = [1:T1, T1:-1:1; r(n).Data(m,:)-squeeze(r(n).expSigma(m,m,:).^.5)'*1, fliplr(r(n).Data(m,:)+squeeze(r(n).expSigma(m,m,:).^.5)'*1)];
-		patch(msh(1,:), msh(2,:), [1 .4 .4],'edgecolor',[1 .7 .7],'edgealpha',.8,'facealpha',.8);
-	end
-	for n=1:nbSamples
-		plot(1:T1, Data(m,(n-1)*T1+1:n*T1), '-','lineWidth',2,'color',[.2 .2 .2]);
-	end
-	for n=1:1
-		plot(1:T1, r(n).Data(m,:), '-','lineWidth',2.5,'color',[.8 0 0]);
-	end
-	
-	set(gca,'xtick',[],'ytick',[]);
-	xlabel('$t$','interpreter','latex','fontsize',18);
-	ylabel(['$x_' num2str(m) '$'],'interpreter','latex','fontsize',18);
-	axis(limAxes);
-end
-
-%% Plot 2D
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-if model.nbVarPos>1
-	figure('position',[620 10 600 600]); hold on;
-	for n=1:1 %nbSamples
-		plotGMM(r(n).Data([1,2],:), r(n).expSigma([1,2],[1,2],:)*2, [1 .2 .2]);
-	end
-	plotGMM(model.Mu([1,2],:), model.Sigma([1,2],[1,2],:)*2, [.5 .5 .5],.8);
-	for n=1:nbSamples
-		plot(Data(1,(n-1)*T1+1:n*T1), Data(2,(n-1)*T1+1:n*T1), '-','lineWidth',2,'color',[.2 .2 .2]); %-0.2+0.8*(n-1)/(nbSamples-1)
-	end
-	for n=1:1
-		plot(r(n).Data(1,:), r(n).Data(2,:), '-','lineWidth',2.5,'color',[.8 0 0]);
-	end
-	set(gca,'xtick',[],'ytick',[]); axis equal; axis square;
-	xlabel(['$x_1$'],'interpreter','latex','fontsize',18);
-	ylabel(['$x_2$'],'interpreter','latex','fontsize',18);
-end
-
-%print('-dpng','graphs/demo_trajGMM01.png');
-%pause;
-%close all;
-
-
diff --git a/m_fcts/DTW.m b/m_fcts/DTW.m
new file mode 100644
index 0000000000000000000000000000000000000000..39ccd5e4fa030d0fa430ecce324bf02ec6c1014e
--- /dev/null
+++ b/m_fcts/DTW.m
@@ -0,0 +1,52 @@
+function [x_new, y_new, p] = DTW(x, y, w)
+%Trajectory realignment through dynamic time warping
+%Sylvain Calinon, 2015
+
+if nargin<3
+  w = Inf;
+end
+
+nx = size(x,2);
+ny = size(y,2);
+
+w = max(w,abs(nx-ny)); 
+
+%Initialization
+D = ones(nx+1,ny+1) * Inf; 
+D(1,1) = 0;
+
+%DP loop
+for i=1:nx
+  for j=max(i-w,1):min(i+w,ny)
+    D(i+1,j+1) = norm(x(:,i)-y(:,j)) + min([D(i,j+1), D(i+1,j), D(i,j)]);
+  end
+end
+
+i=nx+1; j=ny+1;
+p=[];
+while i>1 && j>1
+ [~,id] = min([D(i,j-1), D(i-1,j), D(i-1,j-1)]);
+ if id==1
+   j=j-1;
+ elseif id==2
+   i=i-1;
+ else
+   i=i-1;
+   j=j-1;
+ end
+ p = [p [i;j]];
+end
+
+p = fliplr(p(:,1:end-1)-1);
+
+x_new = x(:,p(1,:));
+y_new = y(:,p(2,:));
+
+%Resampling
+x_new = spline(1:size(x_new,2), x_new, linspace(1,size(x_new,2),nx));
+y_new = spline(1:size(y_new,2), y_new, linspace(1,size(y_new,2),nx));
+
+
+
+
+ 
diff --git a/m_fcts/EM_GMM.m b/m_fcts/EM_GMM.m
index 5492118cd25b0131a93810804f76cd2d7f316c6b..8d9f60ed7b3fb7fd810af7f173f62a6cfaf32084 100644
--- a/m_fcts/EM_GMM.m
+++ b/m_fcts/EM_GMM.m
@@ -30,7 +30,7 @@ for nbIter=1:nbMaxSteps
 		
 		%Update Sigma, see Eq. (2.0.8) in doc/TechnicalReport.pdf (regularization term is optional, see Eq. (2.1.2))
 		DataTmp = Data - repmat(model.Mu(:,i),1,nbData);
-		model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+		model.Sigma(:,:,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(size(Data,1)) * diagRegularizationFactor;
 	end
 	
 	%Compute average log-likelihood
diff --git a/m_fcts/EM_tensorGMM.m b/m_fcts/EM_tensorGMM.m
index 15073fa59f606e49300d1e6e0a1f1e6b10962de1..8e7d31547d5d869bed4882317633d51ecd2d4ac8 100644
--- a/m_fcts/EM_tensorGMM.m
+++ b/m_fcts/EM_tensorGMM.m
@@ -1,4 +1,4 @@
-function [model, GAMMA0] = EM_tensorGMM(Data, model)
+function [model, GAMMA0, GAMMA2] = 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.
@@ -51,7 +51,7 @@ for nbIter=1:nbMaxSteps
 			
 			%Update Sigma (regularization term is optional), see Eq. (6.0.4) in doc/TechnicalReport.pdf
 			DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
-			model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+			model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(size(DataTmp,1)) * diagRegularizationFactor;
 		end
 	end
 	
diff --git a/m_fcts/GPR.m b/m_fcts/GPR.m
index 0044dd338624f41d8c317c896fad3710c640e3e9..7c5dd069e9f56f102117fc49c4b3f79caf60d845 100644
--- a/m_fcts/GPR.m
+++ b/m_fcts/GPR.m
@@ -33,7 +33,7 @@ yd = (Kd * invK * y')'; % + repmat(ymean,1,size(qd,2));
 if nargout>1
 	SigmaOut = zeros(size(yd,1), size(yd,1), size(yd,2));
 	
-% 	%Evaluate Sigma (as in Rasmussen 2006)
+% 	%Evaluate Sigma (as in Rasmussen, 2006)
 % 	Mdd = pdist2(qd',qd');
 % 	Kdd = exp(-Mdd.^2);
 % 	S = Kdd - Kd * invK * Kd';
@@ -41,7 +41,7 @@ if nargout>1
 % 		SigmaOut(:,:,t) = eye(size(yd,1)) * S(t,t); 
 % 	end
 
-	%Evaluate Sigma
+	%Evaluate Sigma (as in GMR)
 	%nbSamples = size(y,2) / size(yd,2);
 	%yd = repmat(yd,1,nbSamples);
 	for t=1:size(yd,2)
diff --git a/m_fcts/constructPHI.m b/m_fcts/constructPHI.m
index a16a87f11160a1b3e930618f5c8720633eff9c32..d8a0f5c8eca8cab6441d1766fd0c7d55a4461d60 100644
--- a/m_fcts/constructPHI.m
+++ b/m_fcts/constructPHI.m
@@ -1,4 +1,4 @@
-function [PHI,PHI1,T,T1] = constructPHI(model,nbD,nbSamples)
+function [PHI,PHI1,PHI0,T,T1] = constructPHI(model,nbD,nbSamples)
 %Construct PHI operator (big sparse matrix) used in trajectory-GMM, see Eq. (2.4.5) in doc/TechnicalReport.pdf
 %Sylvain Calinon, 2015
 
diff --git a/m_fcts/gaussPDF.m b/m_fcts/gaussPDF.m
index 4bc34cb96d7c5fbd805514f2dd4a342ca8d48af5..38e6f464e46b8b99e9f39268aa4a88d2e93098e4 100644
--- a/m_fcts/gaussPDF.m
+++ b/m_fcts/gaussPDF.m
@@ -15,4 +15,4 @@ function prob = gaussPDF(Data, Mu, Sigma)
 %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));
+prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * abs(det(Sigma)) + realmin);
diff --git a/m_fcts/init_GMM_timeBased.m b/m_fcts/init_GMM_timeBased.m
index 0677312ac265ebf01309e948adb1b8086d5a0ca7..574e8537537391bf0a9356a1e61d6d490750f257 100644
--- a/m_fcts/init_GMM_timeBased.m
+++ b/m_fcts/init_GMM_timeBased.m
@@ -22,7 +22,8 @@ function model = init_GMM_timeBased(Data, model)
 %               Switzerland, http://lasa.epfl.ch
 
 [nbVar, nbData] = size(Data);
-diagRegularizationFactor = 1E-2;
+%diagRegularizationFactor = 1E-2;
+diagRegularizationFactor = 1E-8;
 
 TimingSep = linspace(min(Data(1,:)), max(Data(1,:)), model.nbStates+1);
 
diff --git a/m_fcts/init_tensorGMM_kmeans.m b/m_fcts/init_tensorGMM_kmeans.m
index c0ea9ff61817d892d0ee0a0851be6358010ce5c2..50ec07ef95042e4fff9292eb6d969550da8ca831 100644
--- a/m_fcts/init_tensorGMM_kmeans.m
+++ b/m_fcts/init_tensorGMM_kmeans.m
@@ -5,6 +5,7 @@ function model = init_tensorGMM_kmeans(Data, model)
 
 diagRegularizationFactor = 1E-4;
 
+nbVar = size(Data,1);
 DataAll = reshape(Data, size(Data,1)*size(Data,2), size(Data,3)); %Matricization/flattening of tensor
 
 %k-means clustering
@@ -20,8 +21,8 @@ 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);
+		model.Mu(:,m,i) = Mu((m-1)*nbVar+1:m*nbVar,i);
+		model.Sigma(:,:,m,i) = Sigma((m-1)*nbVar+1:m*nbVar,(m-1)*nbVar+1:m*nbVar,i);
 	end
 end
 
diff --git a/m_fcts/kmeansClustering.m b/m_fcts/kmeansClustering.m
index f69d560d3d0f8310c85e7140347eb4545192e803..4483814fc658bff18b2d3e513c8ab6b405eef1c9 100644
--- a/m_fcts/kmeansClustering.m
+++ b/m_fcts/kmeansClustering.m
@@ -8,7 +8,7 @@ cumdist_threshold = 1e-10;
 maxIter = 100;
 
 %Initialization of the parameters
-[nbVar, nbData] = size(Data);
+[~, nbData] = size(Data);
 cumdist_old = -realmax;
 nbStep = 0;
 
@@ -35,10 +35,10 @@ while 1
 	end
 	cumdist_old = cumdist;
 	nbStep = nbStep+1;
-	%   if nbStep>maxIter
-	%     disp(['Maximum number of iterations, ' num2str(maxIter) 'is reached']);
-	%     break;
-	%   end
+	if nbStep>maxIter
+		disp(['Maximum number of kmeans iterations, ' num2str(maxIter) 'is reached']);
+		break;
+	end
 end
 
 %disp(['Kmeans stopped after ' num2str(nbStep) ' steps.']);
diff --git a/m_fcts/plot2DArrow.m b/m_fcts/plot2DArrow.m
index a39b3a019a29408ff70dd032826eb7254e16f7b7..aba232896c0cf32025746fbbfbb6e4c0e00cef92 100644
--- a/m_fcts/plot2DArrow.m
+++ b/m_fcts/plot2DArrow.m
@@ -2,7 +2,7 @@ function h = plot2DArrow(pos,dir,col)
 %Sylvain Calinon, 2015
 
 h(1) = plot([pos(1) pos(1)+dir(1)], [pos(2) pos(2)+dir(2)], '-','linewidth',2,'color',col);
-sz = 8E-2;
+sz = 1E-1;
 pos = pos+dir;
 if norm(dir)>sz
   dir = dir/norm(dir);
diff --git a/m_fcts/plotGMM.m b/m_fcts/plotGMM.m
index 68e9a79535ad7e371c9e435f0041fe1728a62264..faebe32f2d877537131ea670639fc8ca3207f82a 100644
--- a/m_fcts/plotGMM.m
+++ b/m_fcts/plotGMM.m
@@ -11,7 +11,7 @@ function h = plotGMM(Mu, Sigma, color, valAlpha)
 
 nbStates = size(Mu,2);
 nbDrawingSeg = 35;
-lightcolor = min(color+0.5,1);
+darkcolor = color*0.5; %max(color-0.5,0);
 t = linspace(-pi, pi, nbDrawingSeg);
 
 h=[];
@@ -21,11 +21,12 @@ for i=1:nbStates
 	R = real(V*D.^.5);
 	X = R * [cos(t); sin(t)] + repmat(Mu(:,i), 1, nbDrawingSeg);
 	if nargin>3 %Plot with alpha transparency
-		h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color, 'facealpha', valAlpha,'edgealpha', valAlpha)];
-		MuTmp = [cos(t); sin(t)] * 0.006 + repmat(Mu(:,i),1,nbDrawingSeg);
-		h = [h patch(MuTmp(1,:), MuTmp(2,:), color, 'LineStyle', 'none', 'facealpha', valAlpha)];  
+		h = [h patch(X(1,:), X(2,:), color, 'lineWidth', 1, 'EdgeColor', darkcolor, 'facealpha', valAlpha,'edgealpha', valAlpha)];
+		%MuTmp = [cos(t); sin(t)] * 0.3 + repmat(Mu(:,i),1,nbDrawingSeg);
+		%h = [h patch(MuTmp(1,:), MuTmp(2,:), darkcolor, 'LineStyle', 'none', 'facealpha', valAlpha)];
+		h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 6, 'color', darkcolor)];
 	else %Plot without transparency
-		h = [h patch(X(1,:), X(2,:), lightcolor, 'lineWidth', 1, 'EdgeColor', color)];
-		h = [h plot(Mu(1,:), Mu(2,:), '.', 'lineWidth', 2, 'markersize', 6, 'color', color)];
+		h = [h patch(X(1,:), X(2,:), color, 'lineWidth', 1, 'EdgeColor', darkcolor)];
+		h = [h plot(Mu(1,:), Mu(2,:), '.', 'markersize', 6, 'color', darkcolor)];
 	end
 end
diff --git a/m_fcts/reproduction_LQR_finiteHorizon.m b/m_fcts/reproduction_LQR_finiteHorizon.m
index deb647ab929741693285a43078299f3c00edd34b..8808bcd41b7dc77a88284a158dc8b1e0d518d164 100644
--- a/m_fcts/reproduction_LQR_finiteHorizon.m
+++ b/m_fcts/reproduction_LQR_finiteHorizon.m
@@ -1,4 +1,4 @@
-function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor, Pfinal)
+function r = reproduction_LQR_finiteHorizon(model, r, currPos, rFactor, Pfinal)
 % Reproduction with a linear quadratic regulator of finite horizon
 %
 % Authors: Sylvain Calinon and Danilo Bruno, 2014
@@ -17,8 +17,7 @@ function r = reproduction_LQR_finiteHorizon(DataIn, model, r, currPos, rFactor,
 %   pages="3339--3344"
 % }
 
-nbData = size(DataIn,2);
-nbVarOut = model.nbVar - size(DataIn,1);
+[nbVarOut,nbData] = size(r.currTar);
 
 %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t) (See Eq. (5.0.2) in doc/TechnicalReport.pdf)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -46,17 +45,10 @@ L = zeros(nbVarOut, nbVarOut*2, nbData);
 %Compute P_T from the desired final feedback gains L_T,
 P(:,:,nbData) = Pfinal;
 
-%Compute derivative of target path
-%dTar = diff(r.currTar, 1, 2); %For optional feedforward term computation
-
 %Variables for feedforward term computation (optional for movements with low dynamics)
+%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
 tar = [r.currTar; zeros(nbVarOut,nbData)];
 dtar = gradient(tar,1,2)/model.dt;
-%tar = [r.currTar; gradient(r.currTar,1,2)/model.dt];
-%dtar = gradient(tar,1,2)/model.dt;
-%tar = [r.currTar; diff([r.currTar(:,1) r.currTar],1,2)/model.dt];
-%dtar = diff([tar tar(:,1)],1,2)/model.dt;
-
 
 %Backward integration of the Riccati equation and additional equation
 for t=nbData-1:-1:1
@@ -74,19 +66,20 @@ end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 x = currPos;
 dx = zeros(nbVarOut,1);
+
 for t=1:nbData
 	%Compute acceleration (with only feedback term)
-	%ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx];
+	%ddx = -L(:,:,t) * [x-r.currTar(:,t); dx];
 	
 	%Compute acceleration (with both feedback and feedforward terms)
-	ddx =  -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
+	ddx = -L(:,:,t) * [x-r.currTar(:,t); dx] + M(:,t); %See Eq. (5.1.30) in doc/TechnicalReport.pdf
 	
 	%Update velocity and position
 	dx = dx + ddx * model.dt;
 	x = x + dx * model.dt;
 
 	%Log data (with additional variables collected for analysis purpose)
-	r.Data(:,t) = [DataIn(:,t); x];
+	r.Data(:,t) = x;
 	r.ddxNorm(t) = norm(ddx);
 	%r.FB(:,t) = -L(:,:,t) * [x-r.currTar(:,t); dx];
 	%r.FF(:,t) = M(:,t);
@@ -97,5 +90,3 @@ for t=1:nbData
 	%Note that if [V,D] = eigs(L(:,1:nbVarOut)), we have L(:,nbVarOut+1:end) = V * (2*D).^.5 * V'
 end
 
-
-
diff --git a/m_fcts/reproduction_LQR_infiniteHorizon.m b/m_fcts/reproduction_LQR_infiniteHorizon.m
index 2037c6a127d3bbd530001c1de7086799bd0a0059..5a6fd5e35d3c359d420760c565c037bd12efef63 100644
--- a/m_fcts/reproduction_LQR_infiniteHorizon.m
+++ b/m_fcts/reproduction_LQR_infiniteHorizon.m
@@ -1,4 +1,4 @@
-function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFactor)
+function [r,P] = reproduction_LQR_infiniteHorizon(model, r, currPos, rFactor)
 % Reproduction with a linear quadratic regulator of infinite horizon
 %
 % Authors: Sylvain Calinon and Danilo Bruno, 2014
@@ -17,8 +17,7 @@ function [r,P] = reproduction_LQR_infiniteHorizon(DataIn, model, r, currPos, rFa
 %   pages="3339--3344"
 % }
 
-nbData = size(DataIn,2);
-nbVarOut = model.nbVar - size(DataIn,1);
+[nbVarOut,nbData] = size(r.currTar);
 
 %% LQR with cost = sum_t X(t)' Q(t) X(t) + u(t)' R u(t)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -65,17 +64,17 @@ for t=1:nbData
 	M = R\B'*d; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
 	
 	%Compute acceleration (with only feedback terms)
-	%ddx =  -L * [x-r.currTar(:,t); dx];
+	ddx =  -L * [x-r.currTar(:,t); dx];
 	
 	%Compute acceleration (with feedback and feedforward terms)
-	ddx =  -L * [x-r.currTar(:,t); dx] + M; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
+	%ddx =  -L * [x-r.currTar(:,t); dx] + M; %See Eq. (5.1.30) in doc/TechnicalReport.pdf
 	
 	%Update velocity and position
 	dx = dx + ddx * model.dt;
 	x = x + dx * model.dt;
 	
 	%Log data (with additional variables collected for analysis purpose)
-	r.Data(:,t) = [DataIn(:,t); x];
+	r.Data(:,t) = x;
 	r.ddxNorm(t) = norm(ddx);
 	%r.FB(:,t) = -L * [x-r.currTar(:,t); dx];
 	%r.FF(:,t) = M;