Commit 10deffa2 authored by Milad Malekzadeh's avatar Milad Malekzadeh

Some comments added.

parent 1a973baf
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
function model = EM_tensorGMM(Data, model)
% Training of a task-parameterized Gaussian mixture model (GMM) with an expectation-maximization (EM) algorithm.
% The approach allows the modulation of the centers and covariance matrices of the Gaussians with respect to
% external parameters represented in the form of candidate coordinate systems.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
%Parameters of the EM algorithm
nbMinSteps = 5; %Minimum number of iterations allowed
nbMaxSteps = 100; %Maximum number of iterations allowed
maxDiffLL = 1E-4; %Likelihood increase threshold to stop the algorithm
nbData = size(Data,3);
%diagRegularizationFactor = 1E-2;
diagRegularizationFactor = 1E-4;
for nbIter=1:nbMaxSteps
fprintf('.');
%E-step
[L, GAMMA, GAMMA0] = computeGamma(Data, model); %See 'computeGamma' function below and Eq. (2.0.5) in doc/TechnicalReport.pdf
GAMMA2 = GAMMA ./ repmat(sum(GAMMA,2),1,nbData);
%M-step
for i=1:model.nbStates
%Update Priors
model.Priors(i) = sum(sum(GAMMA(i,:))) / nbData; %See Eq. (2.0.6) in doc/TechnicalReport.pdf
for m=1:model.nbFrames
%Matricization/flattening of tensor
DataMat(:,:) = Data(:,m,:);
%Update Mu
model.Mu(:,m,i) = DataMat * GAMMA2(i,:)'; %See Eq. (2.0.7) in doc/TechnicalReport.pdf
%Update Sigma (regularization term is optional)
DataTmp = DataMat - repmat(model.Mu(:,m,i),1,nbData);
model.Sigma(:,:,m,i) = DataTmp * diag(GAMMA2(i,:)) * DataTmp' + eye(model.nbVar) * diagRegularizationFactor; %See Eq. (2.0.8) and (2.1.2) in doc/TechnicalReport.pdf
end
end
%Compute average log-likelihood
LL(nbIter) = sum(log(sum(L,1))) / size(L,2); %See Eq. (2.0.4) in doc/TechnicalReport.pdf
%Stop the algorithm if EM converged (small change of LL)
if nbIter>nbMinSteps
if LL(nbIter)-LL(nbIter-1)<maxDiffLL || nbIter==nbMaxSteps-1
disp(['EM converged after ' num2str(nbIter) ' iterations.']);
return;
end
end
end
disp(['The maximum number of ' num2str(nbMaxSteps) ' EM iterations has been reached.']);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [L, GAMMA, GAMMA0] = computeGamma(Data, model)
%See Eq. (2.0.5) in doc/TechnicalReport.pdf
nbData = size(Data, 3);
L = ones(model.nbStates, nbData);
GAMMA0 = zeros(model.nbStates, model.nbFrames, nbData);
for m=1:model.nbFrames
DataMat(:,:) = Data(:,m,:); %Matricization/flattening of tensor
for i=1:model.nbStates
GAMMA0(i,m,:) = model.Priors(i) * gaussPDF(DataMat, model.Mu(:,m,i), model.Sigma(:,:,m,i));
L(i,:) = L(i,:) .* squeeze(GAMMA0(i,m,:))';
end
end
%Normalization
GAMMA = L ./ repmat(sum(L,1)+realmin,size(L,1),1);
end
......@@ -10,16 +10,19 @@ expData = zeros(nbVarOut,nbData);
expSigma = zeros(nbVarOut,nbVarOut,nbData);
for t=1:nbData
%Compute activation weight
%See Eq. (3.0.5) in doc/TechnicalReport.pdf
for i=1:model.nbStates
H(i,t) = model.Priors(i) * gaussPDF(DataIn(:,t), model.Mu(in,i), model.Sigma(in,in,i));
end
H(:,t) = H(:,t)/sum(H(:,t));
%Compute expected conditional means
%See Eq. (3.0.3) in doc/TechnicalReport.pdf
for i=1:model.nbStates
MuTmp(:,i) = model.Mu(out,i) + model.Sigma(out,in,i)/model.Sigma(in,in,i) * (DataIn(:,t)-model.Mu(in,i));
expData(:,t) = expData(:,t) + H(i,t) * MuTmp(:,i);
end
%Compute expected conditional covariances
%See Eq. (3.0.4) in doc/TechnicalReport.pdf
for i=1:model.nbStates
SigmaTmp = model.Sigma(out,out,i) - model.Sigma(out,in,i)/model.Sigma(in,in,i) * model.Sigma(in,out,i);
expSigma(:,:,t) = expSigma(:,:,t) + H(i,t) * (SigmaTmp + MuTmp(:,i)*MuTmp(:,i)');
......
function r = estimateAttractorPath(DataIn, model, r)
% Estimation of an attractor path from a task-parameterized GMM and a set of candidate frames.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
in = 1:size(DataIn,1);
out = in(end)+1:model.nbVar;
%% Estimation of the attractor path by Gaussian mixture regression,
%% by using the GMM resulting from the product of linearly transformed Gaussians
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[r.Mu, r.Sigma] = productTPGMM(model, r.p);
r.Priors = model.Priors;
r.nbStates = model.nbStates;
[r.currTar, r.currSigma] = GMR(r, DataIn, in, out);
function r = estimateAttractorPath(DataIn, model, r)
% Estimation of an attractor path from a task-parameterized GMM and a set of candidate frames.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
%
% This source code is given for free! In exchange, I would be grateful if you cite
% the following reference in any academic publication that uses this code or part of it:
%
% @inproceedings{Calinon14ICRA,
% author="Calinon, S. and Bruno, D. and Caldwell, D. G.",
% title="A task-parameterized probabilistic model with minimal intervention control",
% booktitle="Proc. {IEEE} Intl Conf. on Robotics and Automation ({ICRA})",
% year="2014",
% month="May-June",
% address="Hong Kong, China",
% pages="3339--3344"
% }
in = 1:size(DataIn,1);
out = in(end)+1:model.nbVar;
%% Estimation of the attractor path by Gaussian mixture regression,
%% by using the GMM resulting from the product of linearly transformed Gaussians
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[r.Mu, r.Sigma] = productTPGMM(model, r.p); %See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
r.Priors = model.Priors;
r.nbStates = model.nbStates;
[r.currTar, r.currSigma] = GMR(r, DataIn, in, out); %See Eq. (3.0.2) to (3.0.5) in doc/TechnicalReport.pdf
function prob = gaussPDF(Data, Mu, Sigma)
% Likelihood of datapoint(s) to be generated by a Gaussian parameterized by center and covariance.
%
% Inputs -----------------------------------------------------------------
% o Data: D x N array representing N datapoints of D dimensions.
% o Mu: D x 1 vector representing the center of the Gaussian.
% o Sigma: D x D array representing the covariance matrix of the Gaussian.
% Output -----------------------------------------------------------------
% o prob: 1 x N vector representing the likelihood of the N datapoints.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
[nbVar,nbData] = size(Data);
Data = Data' - repmat(Mu',nbData,1);
prob = sum((Data/Sigma).*Data, 2);
prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin));
function prob = gaussPDF(Data, Mu, Sigma)
% Likelihood of datapoint(s) to be generated by a Gaussian parameterized by center and covariance.
%
% Inputs -----------------------------------------------------------------
% o Data: D x N array representing N datapoints of D dimensions.
% o Mu: D x 1 vector representing the center of the Gaussian.
% o Sigma: D x D array representing the covariance matrix of the Gaussian.
% Output -----------------------------------------------------------------
% o prob: 1 x N vector representing the likelihood of the N datapoints.
%
% Author: Sylvain Calinon, 2014
% http://programming-by-demonstration.org/SylvainCalinon
[nbVar,nbData] = size(Data);
%See Eq. (2.0.3) in doc/TechnicalReport.pdf
Data = Data' - repmat(Mu',nbData,1);
prob = sum((Data/Sigma).*Data, 2);
prob = exp(-0.5*prob) / sqrt((2*pi)^nbVar * (abs(det(Sigma))+realmin));
......@@ -5,6 +5,7 @@ function [Mu, Sigma] = productTPGMM(model, p)
% set of parameters are stored in the variable 'p'.
% TP-GMM products
%See Eq. (6.0.5), (6.0.6) and (6.0.7) in doc/TechnicalReport.pdf
for i = 1:model.nbStates
% Reallocating
SigmaTmp = zeros(model.nbVar);
......
......@@ -25,9 +25,9 @@ nbVarOut = model.nbVar - size(DataIn,1);
x = currPos;
dx = zeros(nbVarOut,1);
for t=1:nbData
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
L = [eye(nbVarOut)*model.kP, eye(nbVarOut)*model.kV];
%Compute acceleration
ddx = -L * [x-r.currTar(:,t); dx];
ddx = -L * [x-r.currTar(:,t); dx]; %See Eq. (4.0.1) in doc/TechnicalReport.pdf
%Update velocity and position
dx = dx + ddx * model.dt;
x = x + dx * model.dt;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment