Initial commit

EM_tensorGMM.m

+function model = EM_tensorGMM(Data, model)
+% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm. 
+% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to 
+% external parameters represented in the form of candidate coordinate systems.   
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%Parameters of the EM algorithm
+nbMinSteps = 5; %Minimum number of iterations allowed
+nbMaxSteps = 100; %Maximum number of iterations allowed
+maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
+nbData = size(Data,3);
+
+%diagRegularizationFactor = 1E-2;
+diagRegularizationFactor = 1E-4;
+
+for nbIter=1:nbMaxSteps
+  fprintf('.');
+  %E-step
+  [L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below 
+  GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
+  %M-step
+  for i=1:model.nbStates 
+    %Update Priors
+    model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData;
+    for m=1:model.nbFrames
+      %Matricization/flattening of tensor
+      DataMat(:,:) = Data(:,m,:);
+      %Update Mu 
+      model.Mu(:,m,i) = DataMat * GAMMA2(i,:)';
+      %Update Sigma (regularization term is optional) 
+      DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
+      model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor;
+    end
+  end
+  %Compute average log-likelihood 
+  LL(nbIter) = sum(log(sum(L,1))) / size(L,2);
+  %Stop the algorithm if EM converged (small change of LL)
+  if nbIter>nbMinSteps
+    if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
+      disp(['EM converged after ' num2str(nbIter) ' iterations.']); 
+      return;
+    end
+  end
+end
+disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']); 
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [L, GAMMA, GAMMA0] = computeGamma(Data, model)
+  nbData = size(Data, 3);
+  L = ones(model.nbStates, nbData);
+  GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
+  for m=1:model.nbFrames
+    DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
+    for i=1:model.nbStates
+      GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
+      L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))'; 
+    end
+  end
+  %Normalization
+  GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1);
+end
+
+
+
+

demo01.m

+function demo01
+% Demonstration a task-parameterized probabilistic model encoding movements in the form of virtual spring-damper 
+% systems acting in multiple frames of reference. Each candidate coordinate system observes a set of 
+% demonstrations from its own perspective, by extracting an attractor path whose variations depend on the 
+% relevance of the frame through the task. This information is exploited to generate a new attractor path 
+% corresponding to new situations (new positions and orientation of the frames), while the predicted covariances 
+% are exploited by a linear quadratic regulator (LQR) to estimate the stiffness and damping feedback terms of 
+% the spring-damper systems, resulting in a minimal intervention control strategy.
+%
+% Several reproduction algorithms can be selected by commenting/uncommenting lines 89-91 and 110-112
+% (finite/infinite horizon LQR or dynamical system with constant gains).
+%  
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbStates = 3; %Number of Gaussians in the GMM
+model.nbFrames = 2; %Number of candidate frames of reference
+model.nbVar = 3; %Dimension of the datapoints in the dataset (here: t,x1,x2)
+model.dt = 0.01; %Time step 
+nbRepros = 8; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+
+%% Load 3rd order tensor data
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Load 3rd order tensor data...');
+% The MAT file contains a structure 's' with the multiple demonstrations. 's(n).Data' is a matrix data for 
+% sample n (with 's(n).nbData' datapoints). 's(n).p(m).b' and 's(n).p(m).A' contain the position and 
+% orientation of the m-th candidate coordinate system for this demonstration. 'Data' contains the observations 
+% in the different frames. It is a 3rd order tensor of dimension D x P x N, with D=3 the dimension of a 
+% datapoint, P=2 the number of candidate frames, and N=200x4 the number of datapoints in a trajectory (200)
+% multiplied by the number of demonstrations (5).
+load('data/DataLQR01.mat');
+
+
+% %% Optional recomputation of 'Data' (only required when using reproduction_DS)
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% model.kP = 100; %Stiffness gain (required only if LQR is not used for reproduction)
+% model.kV = (2*model.kP)^.5; %Damping gain (required only if LQR is not used for reproduction)
+% nbD = s(1).nbData;
+% nbVarOut = model.nbVar - 1;
+% %Create transformation matrix to compute [X; DX; DDX]
+% D = (diag(ones(1,nbD-1),-1)-eye(nbD)) / model.dt;
+% D(end,end) = 0;
+% %Create transformation matrix to compute XHAT = X + DX*kV/kP + DDX/kP
+% K1d = [1, model.kV/model.kP, 1/model.kP];
+% K = kron(K1d,eye(nbVarOut));
+% %Create 3rd order tensor data with XHAT instead of X
+% for n=1:nbSamples
+%   DataTmp = s(n).Data0(2:end,:);
+%   DataTmp = [s(n).Data0(1,:); K * [DataTmp; DataTmp*D; DataTmp*D*D]];
+%   for m=1:model.nbFrames
+%     Data(:,m,(n-1)*nbD+1:n*nbD) = s(n).p(m).A \ (DataTmp - repmat(s(n).p(m).b, 1, nbD)); 
+%   end
+% end
+
+
+%% Tensor GMM learning
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+fprintf('Parameters estimation of tensor GMM with EM:');
+model = init_tensorGMM_timeBased(Data, model); %Initialization 
+model = EM_tensorGMM(Data, model);
+
+
+%% Reproduction with LQR for the task parameters used to train the model
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:s(1).nbData] * model.dt;
+for n=1:nbSamples
+  %Retrieval of attractor path through task-parameterized GMR
+  a(n) = estimateAttractorPath(DataIn, model, s(n));
+  
+  %Reproduction with one of the selected approach
+  r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+  %r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a(n), a(n).currTar(:,1), rFactor);
+  %r(n) = reproduction_DS(DataIn, model, a(n), a(n).currTar(:,1)); %This function requires to define model.kP and model.kV (see lines 38-39)
+end
+
+
+%% Reproduction with LQR for new task parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('New reproductions with LQR...');
+for n=1:nbRepros 
+  for m=1:model.nbFrames
+    %Random generation of new task parameters 
+    id=ceil(rand(2,1)*nbSamples);
+    w=rand(2); w=w/sum(w);
+    rTmp.p(m).b = s(id(1)).p(m).b * w(1) + s(id(2)).p(m).b * w(2);
+    rTmp.p(m).A = s(id(1)).p(m).A * w(1) + s(id(2)).p(m).A * w(2);
+  end
+  %Retrieval of attractor path through task-parameterized GMR
+  anew(n) = estimateAttractorPath(DataIn, model, rTmp);
+  
+  %Reproduction with one of the selected approach
+  rnew(n) = reproduction_LQR_finiteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+  %rnew(n) = reproduction_LQR_infiniteHorizon(DataIn, model, anew(n), anew(n).currTar(:,1), rFactor);
+  %rnew(n) = reproduction_DS(DataIn, model, anew(n), anew(n).currTar(:,1)); %The fct requires to define model.kP and model.kV (see lines 38-39)
+end
+
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+xx = round(linspace(1,64,nbSamples));
+clrmap = colormap('jet');
+clrmap = min(clrmap(xx,:),.95);
+limAxes = [-1.2 0.8 -1.1 0.9];
+colPegs = [[.9,.5,.9];[.5,.9,.5]];
+
+%DEMOS
+subplot(1,3,1); hold on; box on; title('Demonstrations');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot trajectories
+  plot(s(n).Data0(2,1), s(n).Data0(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(s(n).Data0(2,:), s(n).Data0(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%REPROS
+subplot(1,3,2); hold on; box on; title('Reproductions with LQR');
+for n=1:nbSamples
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([s(n).p(m).b(2) s(n).p(m).b(2)+s(n).p(m).A(2,3)], [s(n).p(m).b(3) s(n).p(m).b(3)+s(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(s(n).p(m).b(2), s(n).p(m).b(3),'.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(r(n).Mu(2:3,:,1), r(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbSamples
+  %Plot trajectories
+  plot(r(n).Data(2,1), r(n).Data(3,1),'.','markersize',12,'color',clrmap(n,:));
+  plot(r(n).Data(2,:), r(n).Data(3,:),'-','linewidth',1.5,'color',clrmap(n,:));
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+%NEW REPROS
+subplot(1,3,3); hold on; box on; title('New reproductions with LQR');
+for n=1:nbRepros
+  %Plot frames
+  for m=1:model.nbFrames
+    plot([rnew(n).p(m).b(2) rnew(n).p(m).b(2)+rnew(n).p(m).A(2,3)], [rnew(n).p(m).b(3) rnew(n).p(m).b(3)+rnew(n).p(m).A(3,3)], '-','linewidth',6,'color',colPegs(m,:));
+    plot(rnew(n).p(m).b(2), rnew(n).p(m).b(3), '.','markersize',30,'color',colPegs(m,:)-[.05,.05,.05]);
+  end
+  %Plot Gaussians
+  plotGMM(rnew(n).Mu(2:3,:,1), rnew(n).Sigma(2:3,2:3,:,1), [.7 .7 .7]);
+end
+for n=1:nbRepros
+  %Plot trajectories
+  plot(rnew(n).Data(2,1), rnew(n).Data(3,1),'.','markersize',12,'color',[.2 .2 .2]);
+  plot(rnew(n).Data(2,:), rnew(n).Data(3,:),'-','linewidth',1.5,'color',[.2 .2 .2]);
+end
+axis(limAxes); axis square; set(gca,'xtick',[],'ytick',[]);
+
+print('-dpng','outTest1.png');
+
+%Plot additional information 
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot strength of the stiffness term
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, rnew(n).kpDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|Kp|');
+
+%Plot accelerations due to feedback and feedforward terms
+figure; hold on;
+n=1; k=1;
+plot(r(n).FB(k,:),'r-','linewidth',2);
+plot(r(n).FF(k,:),'b-','linewidth',2);
+legend('ddx feedback','ddx feedforward');
+xlabel('t'); ylabel(['ddx_' num2str(k)]);
+
+print('-dpng','outTest2.png');
+
+%pause;
+%close all;
+
+

demo_testLQR01.m

+function demo_testLQR01
+% Test of the linear quadratic regulation
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 500; %Number of datapoints
+nbRepros = 3; %Number of reproductions with new situations randomly generated
+rFactor = 1E-2; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+a.currTar = ones(1,nbData); 
+for n=1:nbRepros
+  a.currSigma = ones(1,1,nbData) * 10^(2-n);
+  %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
+  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on;
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+  %Plot trajectories
+  plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot norm of control commands
+subplot(1,2,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(1,2,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kp');
+
+%pause;
+%close all;

demo_testLQR02.m

+function demo_testLQR02
+% Test of the linear quadratic regulation
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 1000; %Number of datapoints
+nbRepros = 1; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+a.currTar = ones(1,nbData);
+a.currSigma = ones(1,1,nbData)/rFactor; %-> LQR with cost X'X + u'u 
+for n=1:nbRepros
+  %r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor);
+  r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on; 
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+  %Plot trajectories
+  plot(r(n).Data(1,:), r(n).Data(2,:), 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot norm of control commands
+subplot(1,3,1); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).ddxNorm, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(1,3,2); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('kp');
+%Plot stiffness/damping ratio (equals to optimal control ratio 1/2^.5)
+subplot(1,3,3); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet./r(n).kvDet, 'k-', 'linewidth', 2);
+end
+xlabel('t'); ylabel('kp/kv');
+
+r(n).kpDet(1)/r(n).kvDet(1) %equals to optimal control ratio 1/2^.5 = 0.7071
+
+%pause;
+%close all;

demo_testLQR03.m

+function demo_testLQR03
+% Comaprison of linear quadratic regulators with finite and infinite time horizons
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+model.nbVar = 2; %Dimension of the datapoints in the dataset (here: t,x1)
+model.dt = 0.01; %Time step 
+nbData = 400; %Number of datapoints
+nbRepros = 2; %Number of reproductions with new situations randomly generated
+rFactor = 1E-1; %Weighting term for the minimization of control commands in LQR
+
+%% Reproduction with LQR 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+disp('Reproductions with LQR...');
+DataIn = [1:nbData] * model.dt;
+%a.currTar = ones(1,nbData); 
+a.currTar = linspace(100,100,nbData); 
+%a.currTar = sin(linspace(0,8*pi,nbData)); 
+
+%a.currSigma = ones(1,1,nbData);
+a.currSigma = ones(1,1,nbData-100) * 100; 
+a.currSigma(:,:,end+1:end+100) = ones(1,1,100) * 1;
+aFinal.currTar = a.currTar(:,end); 
+aFinal.currSigma = a.currSigma(:,:,end);
+    
+for n=1:nbRepros
+  if n==1
+    r(n) = reproduction_LQR_infiniteHorizon(DataIn, model, a, 0, rFactor);
+  else
+    %First call to LQR to get an estimate of the final feedback terms 
+    [~,Sfinal] = reproduction_LQR_infiniteHorizon(DataIn(end), model, aFinal, 0, rFactor);
+    %Second call to LQR with finite horizon 
+    r(n) = reproduction_LQR_finiteHorizon(DataIn, model, a, 0, rFactor, Sfinal);
+  end
+end
+for n=1:nbRepros
+  %Evaluation of determinant (for analysis purpose)
+  for t=1:nbData
+    r(n).detSigma(t) = det(a.currSigma(:,:,t));
+  end
+end
+
+%% Plots
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+figure('position',[20,50,1300,500]);
+hold on; box on;
+%Plot target
+plot(r(1).Data(1,:), a.currTar, 'r-', 'linewidth', 2);
+for n=1:nbRepros
+  %Plot trajectories
+  plot(r(n).Data(1,:), r(n).Data(2,:), '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('x_1');
+
+figure; 
+%Plot variations
+subplot(2,3,[1,4]); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).detSigma, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|\Sigma|');
+%Plot norm of control commands
+subplot(2,3,[2,5]); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).ddxNorm, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('|ddx|');
+%Plot stiffness
+subplot(2,3,3); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kpDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kp');
+%Plot damping
+subplot(2,3,6); hold on;
+for n=1:nbRepros
+  plot(DataIn, r(n).kvDet, '-', 'linewidth', 2, 'color', ones(3,1)*(n-1)/nbRepros);
+end
+xlabel('t'); ylabel('kv');
+
+%pause;
+%close all;
+

estimateAttractorPath.m

+function r = estimateAttractorPath(DataIn, model, r)
+% Estimation of an attractor path from a task-parameterized GMM and a set of candidate frames.
+%
+% Author:	Sylvain Calinon, 2014
+%         http://programming-by-demonstration.org/SylvainCalinon
+%
+% This source code is given for free! In exchange, I would be grateful if you cite  
+% the following reference in any academic publication that uses this code or part of it: 
+%
+% @inproceedings{Calinon14ICRA,
+%   author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
+%   title="A task-parameterized probabilistic model with minimal intervention control",
+%   booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
+%   year="2014",
+%   month="May-June",
+%   address="Hong Kong, China",
+%   pages="3339--3344"
+% }
+
+nbData = size(DataIn,2);
+in = 1:size(DataIn,1);
+out = in(end)+1:model.nbVar;
+nbVarOut = length(out);
+
+%% GMR to estimate attractor path and associated variations
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%GMM products 
+for i=1:model.nbStates 
+  SigmaTmp = zeros(model.nbVar);
+  MuTmp = zeros(model.nbVar,1);
+  for m=1:model.nbFrames 
+    MuP = r.p(m).A * model.Mu(:,m,i) + r.p(m).b; 
+    SigmaP = r.p(m).A * model.Sigma(:,:,m,i) * r.p(m).A'; 
+    SigmaTmp = SigmaTmp + inv(SigmaP);
+    MuTmp = MuTmp + SigmaP\MuP; 
+  end
+  r.Sigma(:,:,i) = inv(SigmaTmp);
+  r.Mu(:,i) = r.Sigma(:,:,i) * MuTmp;
+end
+
+%GMR
+for t=1:nbData
+  %Compute activation weight
+  for i=1:model.nbStates
+    r.H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), r.Mu(in,i), r.Sigma(in,in,i)); 
+  end
+  r.H(:,t) = r.H(:,t)/sum(r.H(:,t));
+  %Evaluate the current target 
+  currTar = zeros(nbVarOut,1);
+  currSigma = zeros(nbVarOut,nbVarOut); 
+  for i=1:model.nbStates
+    tarTmp = r.Mu(out,i) + r.Sigma(out,in,i)/r.Sigma(in,in,i) * (DataIn(:,t)-r.Mu(in,i)); 
+    SigmaTmp = r.Sigma(out,out,i) - r.Sigma(out,in,i)/r.Sigma(in,in,i) * r.Sigma(in,out,i);
+    currTar = currTar + r.H(i,t) * tarTmp; 
+    currSigma = currSigma + r.H(i,t) * SigmaTmp; %r.H(i,t)^2
+  end
+  r.currTar(:,t) = currTar; 
+  r.currSigma(:,:,t) = currSigma;
+end
+

gaussPDF.m

+function prob = gaussPDF(Data, Mu, Sigma)
+% Likelihood of datapoint(s) to be generated by a Gaussian parameterized by center and covariance.
+%
+% Inputs -----------------------------------------------------------------
+%   o Data:  D x N array representing N datapoints of D dimensions.
+%   o Mu:    D x 1 vector representing the center of the Gaussian.