diff --git a/README.md b/README.md
index 4f8ba1dda6a9de6f1992bd4e1dd5b3b26d7f27c9..fba8b14b746e2f6de6637af31ab1804da82b25f0 100644
--- a/README.md
+++ b/README.md
@@ -364,4 +364,3 @@ PbDlib is free software: you can redistribute it and/or modify it under the term
 PbDlib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
-
diff --git a/demos/demo_Bezier01.m b/demos/demo_Bezier01.m
index 14a98078bd098178626e081aa8ad685eeca1da81..e09075f1a55282ed654bdcbb4e9d21a3818472f6 100644
--- a/demos/demo_Bezier01.m
+++ b/demos/demo_Bezier01.m
@@ -36,18 +36,49 @@ nbDeg = 3; %Degree of the Bezier curve
 nbData = 100; %Number of datapoints in a trajectory
 
 p = rand(nbVar,nbDeg+1); %Control points
-t = linspace(0,1,nbData); %Time range
+tl = linspace(0,1,nbData); %Time range
 
 
 %% Bezier curve as a superposition of Bernstein polynomials
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 B = zeros(nbDeg,nbData);
 for i=0:nbDeg
-	B(i+1,:) = factorial(nbDeg) ./ (factorial(i) .* factorial(nbDeg-i)) .* (1-t).^(nbDeg-i) .* t.^i; %Bernstein basis functions (Eq.(12)
+	B(i+1,:) = factorial(nbDeg) ./ (factorial(i) .* factorial(nbDeg-i)) .* (1-tl).^(nbDeg-i) .* tl.^i; %Bernstein basis functions (Eq.(12)
 end
 x = p * B; %Reconstruction of signal
 
 
+%% Bezier curve computed recursively
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% pTmp = zeros(nbDeg, nbDeg, nbVar, nbData);
+% for t=1:nbData
+% 	for i=1:nbDeg
+% 		for j=i:nbDeg
+% 			if i==1
+% 				pTmp(i,j,:,t) = p(:,j) * (1-tl(t)) + p(:,j+1) * tl(t);
+% 			else
+% 				pTmp(i,j,:,t) = pTmp(i-1,j-1,:,t) * (1-tl(t)) + pTmp(i-1,j,:,t) * tl(t);
+% 			end
+% 		end
+% 	end
+% end
+% x(:,:) = pTmp(end,end,:,:);
+
+% Btmp = zeros(nbDeg, nbDeg+1, nbData);
+% for t=1:nbData
+% 	Btmp(1,1,t) = tl(t);
+% 	Btmp(1,2,t) = 1 - tl(t);
+% 	for i=2:nbDeg
+% 		for j=1:i
+% 			[i, j]
+% 			Btmp(i,j,t) = Btmp(i-1,j,t) * (1-tl(t)) + Btmp(i-1,j+1,t) * tl(t);
+% 		end
+% 	end
+% end
+% B(:,:) = Btmp(end,:,:);
+% x = p * B; %Reconstruction of signal
+
+
 %% Plot
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[10,10,800,800]); 
@@ -60,7 +91,7 @@ end
 plot(x(1,:), x(2,:), '-','linewidth',3,'color',[0 0 0]);
 subplot(2,1,2); hold on;
 for i=0:nbDeg
-	plot(t, B(i+1,:),'linewidth',3);
+	plot(tl, B(i+1,:),'linewidth',3);
 end
 set(gca,'xtick',[0,1],'ytick',[0,1],'fontsize',16);
 xlabel('t','fontsize',18); ylabel('b_i','fontsize',18);
diff --git a/demos/demo_Bezier_illustr01.m b/demos/demo_Bezier_illustr01.m
index 783f8c0f6aa2e42199a4dc793ca0243846338e4d..07f9e0928321075447a0a6179135306a065c859c 100644
--- a/demos/demo_Bezier_illustr01.m
+++ b/demos/demo_Bezier_illustr01.m
@@ -42,7 +42,7 @@ demos=[];
 load('data/2Dletters/N.mat');
 x = spline(1:size(demos{1}.pos,2), demos{1}.pos, linspace(1,size(demos{1}.pos,2),nbData)); %Resampling
 
-figure('position',[10,10,2300,800]); 
+figure('position',[10,10,1300,800]); 
 
 
 %% Control points extraction
diff --git a/demos/demo_GaussProd4nullspace_2D01.m b/demos/demo_GaussProd4nullspace_2D01.m
index 4e42673b4fa7177ffdc9e8bd154216045f771b32..67524bad2dc862e992c18bf0b45de8bf640e5d90 100644
--- a/demos/demo_GaussProd4nullspace_2D01.m
+++ b/demos/demo_GaussProd4nullspace_2D01.m
@@ -38,15 +38,15 @@ addpath('./m_fcts/');
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Mu1 = zeros(2,1);
 % Q1 = [0 0; 0 1E3]
-d = [.5; 1.2] .* 1E2;
-Q1 = d * d';
+d = [.5; 1.2] .* 1E1;
+Q1 = d * d' + eye(2).*1E-1;
 
 % inv(Q1+eye(2)*1E-12)
 % pinv(Q1+eye(2)*1E-12)
 % [SJ,VJ] = eig(Q1);
 % SJ(SJ>0) = SJ(SJ>0).^-1;
 % S1 = VJ * SJ * VJ'
-S1 = inv(Q1 + eye(2).*1E-8);
+S1 = inv(Q1);
 
 Mu2 = [1; 2];
 d2 = [.05; -.2] .* 8;
diff --git a/demos/demo_IK01.m b/demos/demo_IK01.m
index 0521ea1adfd8c36354a9d9682a40020274977c20..1926d4a94c72e871c5c5320079dbbcf8269236d7 100644
--- a/demos/demo_IK01.m
+++ b/demos/demo_IK01.m
@@ -37,11 +37,11 @@ addpath('./m_fcts/');
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 dt = 1E-2; %Time step
 nbData = 100;
- use 
+
 
 %% Robot parameters 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbDOFs = 2; %Number of articulations
+model.nbDOFs = 3; %Number of articulations
 model.l = 0.6; %Length of each link
 q(:,1) = [pi/2; pi/2; pi/3]; %Initial pose
 % q = rand(model.nbDOFs,1);
@@ -53,6 +53,7 @@ dxh = [.1; 0];
 for t=1:nbData-1
 	J = jacob0(q(:,t), model); %Jacobian
 	dq = pinv(J) * dxh;
+% 	dq = J'/(J*J') * dxh;
 	q(:,t+1) = q(:,t) + dq * dt;
 	x(:,t+1) = fkine(q(:,t+1), model); %FK for a planar robot
 end
@@ -64,7 +65,7 @@ figure('position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
 for t=floor(linspace(1,nbData,2))
 	colTmp = [.7,.7,.7] - [.7,.7,.7] * t/nbData;
 	plotArm(q(:,t), ones(model.nbDOFs,1)*model.l, [0;0;-2+t/nbData], .03, colTmp);
-	plot(x(1,t), x(2,t), '.','markersize',50,'color',[.8 0 0]);
+% 	plot(x(1,t), x(2,t), '.','markersize',50,'color',[.8 0 0]);
 end
 plot(x(1,:), x(2,:), '.','markersize',20,'color',[0 0 0]);
 axis equal; axis tight;
@@ -78,13 +79,13 @@ end
 %Forward kinematics
 function [x, Tf] = fkine(q, model)
 	Tf = eye(4);
-	T = repmat(Tf, [1,1,size(q,1)]);
-	for n=1:size(q,1)
+	T = repmat(Tf, [1,1,model.nbDOFs]);
+	for n=1:model.nbDOFs
 		c = cos(q(n));
 		s = sin(q(n));
 		T(:,:,n) = [c, -s, 0, model.l * c; ...
 								s, c, 0, model.l * s; ...
-								0, 0, 1, 0;
+								0, 0, 1, 0; ...
 								0, 0, 0, 1]; %Homogeneous matrix 
 		Tf = Tf * T(:,:,n);
 	end
@@ -95,8 +96,8 @@ end
 %Jacobian with numerical computation
 function J = jacob0(q, model)
 	e = 1E-6;
-	J = zeros(2,size(q,1));
-	for n=1:size(q,1)
+	J = zeros(2,model.nbDOFs);
+	for n=1:model.nbDOFs
 		qtmp = q;
 		qtmp(n) = qtmp(n) + e;
 		J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
diff --git a/demos/demo_IK02.m b/demos/demo_IK02.m
index d1198fac12c7d14129ae34d77b9c384cc7039128..fb21f83349755907cb377f8ac22d5a6b01fa8d7f 100644
--- a/demos/demo_IK02.m
+++ b/demos/demo_IK02.m
@@ -1,5 +1,5 @@
 function demo_IK02
-% Inverse kinematics with nullspace projection operator.
+% Inverse kinematics with nullspace projection operator (see also demo_IK_nullspace01.m).
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Silverio19TRO,
@@ -132,7 +132,7 @@ function [x, Tf] = fkine(q, model)
 		s = sin(q(n));
 		T(:,:,n) = [c, -s, 0, model.l * c; ...
 								s, c, 0, model.l * s; ...
-								0, 0, 1, 0;
+								0, 0, 1, 0; ...
 								0, 0, 0, 1]; %Homogeneous matrix 
 		Tf = Tf * T(:,:,n);
 	end
diff --git a/demos/demo_Kalman01.m b/demos/demo_Kalman01.m
index 7e8c25f64bc7ed0c7bcb740168725d3d216c486e..5eb7fa7d44da3e61d40530c7089c615287fb67e3 100644
--- a/demos/demo_Kalman01.m
+++ b/demos/demo_Kalman01.m
@@ -143,7 +143,7 @@ end
 
 %% Plot 2D
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 2200 1200],'color',[1 1 1]);
+figure('position',[10 10 1200 800],'color',[1 1 1]);
 subplot(nbVarPos+1,1,1); hold on; axis off;
 h(1) = plot(xd(1,:), xd(2,:), '-','linewidth',2,'color',[.7 .7 .7]);
 h(2) = plot(r.y(1,:), r.y(2,:), '-','linewidth',1,'color',[.2 .2 .2]);
diff --git a/demos/demo_Riemannian_S1_interp02.m b/demos/demo_Riemannian_S1_interp02.m
index bec2ebb0fb4193201e13c0e1b7a901468ccec78e..a6d1ea5fa4e467f9325689caa922afb967e15318 100644
--- a/demos/demo_Riemannian_S1_interp02.m
+++ b/demos/demo_Riemannian_S1_interp02.m
@@ -72,12 +72,12 @@ end
 
 %% Functions
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-function x = expmap(u,x0)
+function x = expmap(u, x0)
 % 	x = exp(x0*1i) * exp(u*1i);
 	x = x0 + u;
 end
 
-function u = logmap(x,x0)
+function u = logmap(x, x0)
 % 	u = x - x0;
 	u = imag(log(exp(x0*1i)' * exp(x*1i)));
 end
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m
index e3b36783bafec19927ecd0b14b455465b048f72c..a2e1ca4f820c6b9e7a4a323bc1353ad65a91e153 100644
--- a/demos/demo_ergodicControl_1D01.m
+++ b/demos/demo_ergodicControl_1D01.m
@@ -36,9 +36,9 @@ addpath('./m_fcts/');
 
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 4000; %Number of datapoints
-nbFct = 20; %Number of basis functions along x and y
-nbStates = 2; %Number of Gaussians to represent the spatial distribution
+nbData = 5000; %Number of datapoints
+nbFct = 10; %Number of basis functions along x and y
+nbStates = 8; %Number of Gaussians to represent the spatial distribution
 dt = 1E-2; %Time step
 xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
@@ -46,10 +46,12 @@ om = 2 * pi / L; %omega
 u_max = 1E0; %Maximum speed allowed 
 
 %Desired spatial distribution represented as a mixture of Gaussians
-Mu(:,1) = 0.7;
-Sigma(:,1) = 0.003; 
-Mu(:,2) = 0.5;
-Sigma(:,2) = 0.01;
+% Mu(:,1) = 0.7;
+% Sigma(:,1) = 0.003; 
+% Mu(:,2) = 0.5;
+% Sigma(:,2) = 0.01;
+Mu = rand(1, nbStates);
+Sigma = repmat(1E-1, [1,1,nbStates]);
 Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
 
 
@@ -59,15 +61,18 @@ rg = [0:nbFct-1]';
 kk = rg .* om;
 Lambda = (rg.^2 + 1).^-1; %Weighting vector (Eq.(15)
 
+% tic
 %Explicit description of w_hat by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 w_hat = zeros(nbFct,1);
 for j=1:nbStates
 	w_hat = w_hat + Priors(j) .* cos(kk .* Mu(:,j)) .* exp(-.5 * kk.^2 .* Sigma(:,j)); %Eq.(22)
 end
 w_hat = w_hat / L;
+% toc
+% return
 
 % %Alternative computation of w_hat by discretization (for verification)
-% nbRes = 1000;
+% nbRes = 100;
 % x = linspace(xlim(1), xlim(2), nbRes); %Spatial range
 % g = zeros(1,nbRes);
 % for k=1:nbStates
@@ -82,6 +87,7 @@ w_hat = w_hat / L;
 % % end
 % % w_hat = w_hat ./ L ./ nbRes; %Fourier coefficients of spatial distribution
 
+
 %Fourier basis functions (for a discretized map)
 nbRes = 200;
 xm = linspace(xlim(1), xlim(2), nbRes); %Spatial range
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index 9b7ecafecf58312fed06429b754e2039972c0222..3bec9a3108493fc5becda138f7e8b1f1472cdf82 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -36,15 +36,15 @@ addpath('./m_fcts/');
 
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 4000; %Number of datapoints
-nbFct = 6; %Number of basis functions along x and y
+nbData = 2000; %Number of datapoints
+nbFct = 10; %Number of basis functions along x and y
 nbVar = 2; %Dimension of datapoint
 nbStates = 2; %Number of Gaussians to represent the spatial distribution
 sp = (nbVar + 1) / 2; %Sobolev norm parameter
 dt = 1E-2; %Time step
-xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
+xlim = [0; .5]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
-om = 2 .* pi ./ L; %omega
+om = 2 * pi / L; %omega
 u_max = 1E1; %Maximum speed allowed 
 
 %Desired spatial distribution represented as a mixture of Gaussians
@@ -52,10 +52,18 @@ u_max = 1E1; %Maximum speed allowed
 % Sigma(:,:,1) = eye(nbVar).*1E-2; 
 % Mu(:,2) =  [.7; .4]; 
 % Sigma(:,:,2) = [.1;.2]*[.1;.2]' .*8E-1 + eye(nbVar)*1E-3; 
+
 Mu(:,1) = [.5; .7]; 
 Sigma(:,:,1) = [.3;.1]*[.3;.1]' .*5E-1 + eye(nbVar)*5E-3; %eye(nbVar).*1E-2; 
 Mu(:,2) =  [.6; .3]; 
-Sigma(:,:,2) = [.1;.2]*[.1;.2]' .*3E-1 + eye(nbVar)*1E-2; 
+Sigma(:,:,2) = [.1;.2]*[.1;.2]' .*3E-1 + eye(nbVar)*1E-2;
+
+Mu = Mu * 0.5;
+Sigma = Sigma * 0.5;
+
+% Mu = rand(nbVar, nbStates);
+% Sigma = repmat(eye(nbVar)*1E-1, [1,1,nbStates]);
+
 Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
 
 
@@ -66,12 +74,13 @@ rg = 0:nbFct-1;
 [KX(1,:,:), KX(2,:,:)] = ndgrid(rg, rg);
 Lambda = (KX(1,:).^2 + KX(2,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15))
 
+% tic
 %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1])
 op = hadamard(2^(nbVar-1));
 op = op(1:nbVar,:);
 %Compute phi_k
-kk = KX(:,:) .* om;
+kk = KX(:,:) * om;
 w_hat = zeros(nbFct^nbVar, 1);
 for j=1:nbStates
 	for n=1:size(op,2)
@@ -80,13 +89,18 @@ for j=1:nbStates
 		w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
 	end
 end
-w_hat = w_hat ./ L^nbVar ./ size(op,2);
+w_hat = w_hat / L^nbVar / size(op,2);
+% toc
+% return
+
+%Suhan: The summation goes from $m \in \{ 1,...,2^{D-1}\}$. But, H is hadamard matrix of size $2^D$, 
+%and you also write $A_m$ m-th column's last $D$ elements of $H$. I think something is not clear here.
+	
 
 % %Alternative computation of w_hat by discretization (for verification)
 % nbRes = 100;
 % xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
 % [xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
-% phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* 2^nbVar; %Fourier basis functions
 % g = zeros(1,nbRes^nbVar);
 % for k=1:nbStates
 % 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
@@ -94,6 +108,7 @@ w_hat = w_hat ./ L^nbVar ./ size(op,2);
 % phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) ./ L^nbVar ./ nbRes^nbVar;
 % w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
 
+
 %Fourier basis functions (for a discretized map)
 nbRes = 100;
 xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
@@ -144,6 +159,7 @@ for t=1:nbData
 % 	r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
 end
 
+
 % %The Fourier series coefficients along trajectory can alternatively be computed in batch form
 % phi1 = [];
 % size(cos(r.x(1:2:end)' * rg .* om))
@@ -156,7 +172,7 @@ end
 
 %% Plot (static)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,2200,800]); 
+figure('position',[10,10,1400,800]); 
 colormap(repmat(linspace(1,.4,64),3,1)');
 
 %x
diff --git a/demos/demo_ergodicControl_3D01.m b/demos/demo_ergodicControl_3D01.m
index 4f30f046f21690bddd9a188090617d59b01bcdaf..6c73a402a69690248fc0a116dd2515833c907273 100644
--- a/demos/demo_ergodicControl_3D01.m
+++ b/demos/demo_ergodicControl_3D01.m
@@ -36,22 +36,24 @@ addpath('./m_fcts/');
 
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 4000; %Number of datapoints
-nbFct = 20; %Number of basis functions along x and y
+nbData = 5000; %Number of datapoints
+nbFct = 10; %Number of basis functions along x and y
 nbVar = 3; %Dimension of datapoint
-nbStates = 2; %Number of Gaussians to represent the spatial distribution
+nbStates = 8; %Number of Gaussians to represent the spatial distribution
 sp = (nbVar + 1) / 2; %Sobolev norm parameter
 dt = 1E-2; %Time step
 xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
-om = 2 .* pi ./ L; %omega
+om = 2 * pi / L; %omega
 u_max = 1E1; %Maximum speed allowed 
 
 %Desired spatial distribution represented as a mixture of Gaussians
-Mu(:,1) = [.4; .5; .7]; 
-Sigma(:,:,1) = eye(nbVar).*1E-2; 
-Mu(:,2) =  [.7; .4; .3]; 
-Sigma(:,:,2) = [.1;.2;.3]*[.1;.2;.3]' .*8E-1 + eye(nbVar)*1E-3; 
+% Mu(:,1) = [.4; .5; .7]; 
+% Sigma(:,:,1) = eye(nbVar).*1E-2; 
+% Mu(:,2) =  [.7; .4; .3]; 
+% Sigma(:,:,2) = [.1;.2;.3]*[.1;.2;.3]' .*8E-1 + eye(nbVar)*1E-3; 
+Mu = rand(nbVar, nbStates);
+Sigma = repmat(eye(nbVar)*1E-1, [1,1,nbStates]);
 Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
 
 
@@ -66,12 +68,13 @@ Lambda = (KX(1,:).^2 + KX(2,:).^2 + KX(3,:).^2 + 1)'.^-sp; %Weighting vector (Eq
 % hk = [1; sqrt(.5)*ones(nbFct-1,1)];
 % HK = hk(xx,1) .* hk(yy,1) .* hk(zz,1); %Rescaling term (as normalizing matrix)
 
+% tic
 %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1])
 op = hadamard(2^(nbVar-1));
 op = op(1:nbVar,:);
 %Compute phi_k
-kk = KX(:,:) .* om;
+kk = KX(:,:) * om;
 w_hat = zeros(nbFct^nbVar, 1);
 for j=1:nbStates
 	for n=1:size(op,2)
@@ -80,20 +83,26 @@ for j=1:nbStates
 		w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
 	end
 end
-w_hat = w_hat ./ L^nbVar ./ size(op,2);
+w_hat = w_hat / L^nbVar / size(op,2);
+% toc
+% return
 
 % %Alternative computation of w_hat by discretization (for verification)
-% nbRes = 20;
+% nbRes = 100;
 % xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
 % [xm(1,:,:,:), xm(2,:,:,:), xm(3,:,:,:)] = ndgrid(xm1d, xm1d, xm1d); %Spatial range
-% phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) .* 2^nbVar; %Fourier basis functions
 % g = zeros(1,nbRes^nbVar);
 % for k=1:nbStates
 % 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
 % end
 % phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) ./ L^nbVar ./ nbRes^nbVar;
+% % phi_inv = ones(nbFct^nbVar, nbRes^nbVar);
+% % for n=1:nbVar
+% % 	phi_inv = phi_inv .* cos(KX(n,:)' * xm(n,:) .* om) ./ L^nbVar ./ nbRes^nbVar;
+% % end
 % w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
 
+
 %Fourier basis functions (for a discretized map)
 nbRes = 20;
 xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
@@ -143,7 +152,7 @@ end
 
 %% Plot
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1200,1200]); hold on; axis on; box on; rotate3d on;
+figure('position',[10,10,800,800]); hold on; axis on; box on; rotate3d on;
 % colormap(repmat(linspace(1,0,64),3,1)');
 colormap hsv;
 
diff --git a/demos/demo_ergodicControl_nD01.m b/demos/demo_ergodicControl_nD01.m
index 6fcbfbeb79df9b28de11f7637a4d8c07c084bc76..4d608188a2ff2fe6b8058bc910b2be1850b17036 100644
--- a/demos/demo_ergodicControl_nD01.m
+++ b/demos/demo_ergodicControl_nD01.m
@@ -36,23 +36,25 @@ addpath('./m_fcts/');
 
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 2000; %Number of datapoints
+nbData = 5000; %Number of datapoints
 nbFct = 5; %Number of basis functions along x and y
-nbVar = 4; %Dimension of datapoint
+nbVar = 6; %Dimension of datapoint
 nbStates = 2; %Number of Gaussians to represent the spatial distribution
 sp = (nbVar + 1) / 2; %Sobolev norm parameter
 dt = 1E-2; %Time step
 xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
-om = 2 .* pi ./ L; %omega
+om = 2 * pi / L; %omega
 u_max = 5E1; %Maximum speed allowed 
 
 %Desired spatial distribution represented as a mixture of Gaussians
-Mu = rand(nbVar,nbStates);
-Sigma = zeros(nbVar,nbVar,nbStates);
+Mu = rand(nbVar, nbStates);
+Sigma = zeros(nbVar, nbVar, nbStates);
 for n=1:nbStates
 	Sigma(:,:,n) = cov(rand(10,nbVar));
 end
+% Mu = rand(nbVar, nbStates);
+% Sigma = repmat(eye(nbVar)*1E-1, [1,1,nbStates]);
 Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
 
 
@@ -74,6 +76,7 @@ Lambda = (stmp + 1).^-sp; %Weighting vector (Eq.(15))
 % % 	HK = HK .* hk(arr(n).x(:),1); %Rescaling term (as normalizing matrix)
 % % end
 
+tic
 %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1])
 op = hadamard(2^(nbVar-1));
@@ -81,7 +84,7 @@ op = op(1:nbVar,:);
 %Compute phi_k
 kk = [];
 for n=1:nbVar
-	kk = [kk; Karr(n).x(:)' .* om];
+	kk = [kk; Karr(n).x(:)' * om];
 end
 w_hat = zeros(nbFct^nbVar, 1);
 for j=1:nbStates
@@ -91,9 +94,29 @@ for j=1:nbStates
 		w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
 	end
 end
-w_hat = w_hat ./ L^nbVar ./ size(op,2);
+w_hat = w_hat / L^nbVar / size(op,2);
+toc
 
+% %Alternative computation of w_hat by discretization (for verification)
+% nbRes = 10;
+% xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
+% [KX(1,:,:,:,:), KX(2,:,:,:,:), KX(3,:,:,:,:), KX(4,:,:,:,:)] = ndgrid(rg, rg, rg, rg); %for nbVar=4
+% [xm(1,:,:,:,:), xm(2,:,:,:,:), xm(3,:,:,:,:), xm(4,:,:,:,:)] = ndgrid(xm1d, xm1d, xm1d, xm1d); %Spatial range (for nbVar=4)
+% xmarr = ndarr(xm1d, nbVar); %Spatial range (for nbVar>4)
+% g = zeros(1, nbRes^nbVar);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
+% end
+% % phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om)
+% % .* cos(KX(3,:)' * xm(3,:) .* om) .* cos(KX(4,:)' * xm(4,:) .* om) ./ L^nbVar ./ nbRes^nbVar; %for nbVar=4
+% phi_inv = ones(nbFct^nbVar, nbRes^nbVar);
+% for n=1:nbVar
+% 	phi_inv = phi_inv .* cos(KX(n,:)' * xm(n,:) .* om) ./ L^nbVar ./ nbRes^nbVar; %for nbVar=4
+% % 	phi_inv = phi_inv .* cos(Karr(n).x(:) * xmarr(n).x(:)' .* om) ./ L^nbVar ./ nbRes^nbVar; %for nbVar>4
+% end
+% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
 
+tic
 %% Ergodic control 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 x = rand(nbVar,1); %Initial position
@@ -128,11 +151,12 @@ for t=1:nbData
 	
 	x = x + u .* dt; %Update of position
 end
+toc
 
 
 %% Plot
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1200,1200]); hold on; axis off; rotate3d on;
+figure('position',[10,10,1200,800]); hold on; axis off; rotate3d on;
 plotGMM3D(Mu(1:3,:), Sigma(1:3,1:3,:), [.2 .2 .2], .3, 2);
 plot3(Mu(1,:), Mu(2,:), Mu(3,:), '.','markersize',15,'color',[0 0 0]);
 plot3(r.x(1,:), r.x(2,:), r.x(3,:), '-','linewidth',1,'color',[0 0 0]);
diff --git a/demos/demo_proMP_Fourier01.m b/demos/demo_proMP_Fourier01.m
index 72c064db0ddee0b1e514ad521b2daa0fe6f8376a..380966ba20d2ec3621ce57706a0396689f50fcf6 100644
--- a/demos/demo_proMP_Fourier01.m
+++ b/demos/demo_proMP_Fourier01.m
@@ -53,7 +53,7 @@ end
 k = -5:5;
 nbFct = length(k);
 t = linspace(0,1,nbData);
-Psi = exp(t' * k * 2 * pi * 1i) ./ nbData;
+Psi = exp(t' * k * 2 * pi * 1i) / nbData;
 % w = (Psi' * Psi + eye(nbStates).*1E-18) \ Psi' * x; 
 w = pinv(Psi) * x;
 
@@ -74,8 +74,7 @@ Sigma = Psi * Sigma_w * Psi';
 %% Plot 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 clrmap = lines(nbFct);
-figure('position',[10 10 1300 1300]); 
-
+figure('position',[10 10 800 800]); 
 %Plot signal
 subplot(3,1,1); hold on; %axis off;
 for n=1:nbSamples 
@@ -92,7 +91,6 @@ axis([1, nbData, min(msh(2,:))-.5, max(msh(2,:))+.5]); %axis tight;
 set(gca,'xtick',[],'ytick',[]);
 xlabel('$t$','interpreter','latex','fontsize',25); ylabel('$x$','interpreter','latex','fontsize',25);
 legend(h,{'Re($\mu^x$)','Im($\mu^x$)'},'interpreter','latex','location','southeast','fontsize',22);
-
 %Plot basis functions
 subplot(3,1,2); hold on; %axis off; %real part
 for i=1:nbFct
@@ -111,7 +109,7 @@ set(gca,'xtick',[],'ytick',[]);
 xlabel('$t$','interpreter','latex','fontsize',25); ylabel('Im($\phi_k$)','interpreter','latex','fontsize',25);
 
 %Plot magnitude and phase 
-figure('position',[1320 10 800 800]); hold on; axis off;
+figure('position',[820 10 800 800]); hold on; axis off;
 plot2DArrow([0;0], [.7;0].*nbData, [0,0,0], 1, .03.*nbData);
 plot2DArrow([0;0], [0;.7].*nbData, [0,0,0], 1, .03.*nbData);
 text((.7-.15).*nbData, .05.*nbData, 'Re($w$)','interpreter','latex','fontsize',20); text(.02.*nbData, (.7-.1).*nbData, 'Im($w$)','interpreter','latex','fontsize',20);
diff --git a/demos/demo_proMP_Fourier02.m b/demos/demo_proMP_Fourier02.m
index 1f40354dafca74654ed5c8c100204cd0f6242806..49f20cfae1674b78d21689a6875ad30c2572e57b 100644
--- a/demos/demo_proMP_Fourier02.m
+++ b/demos/demo_proMP_Fourier02.m
@@ -55,7 +55,7 @@ end
 k = -5:5;
 nbFct = length(k);
 t = linspace(0,1,nbData);
-phi = exp(t' * k * 2 * pi * 1i) ./ nbData;
+phi = exp(t' * k * 2 * pi * 1i) / nbData;
 Psi = kron(phi, eye(nbVar)); 
 
 % w = (Psi' * Psi + eye(nbFct).*1E-18) \ Psi' * x; 
@@ -90,7 +90,7 @@ xr = Psi * xr_w; %real(2 * Psi(:,2:end) * xr_w(2:end,:)) + Psi(:,1) * xr_w(1,:);
 
 %% Plot 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10 10 1600 1000]); 
+figure('position',[10 10 1200 800]); 
 clrmap = lines(nbFct);
 
 %Plot signal 2D
diff --git a/demos/demo_proMP_Fourier_sampling01.m b/demos/demo_proMP_Fourier_sampling01.m
index 6fa62e48148db01a0a5cb2cf206a558d8517a54f..8cd46802d0c7850e31e91098c9345959cd302e0e 100644
--- a/demos/demo_proMP_Fourier_sampling01.m
+++ b/demos/demo_proMP_Fourier_sampling01.m
@@ -104,7 +104,7 @@ xr = Psi * xw;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 clrmap = lines(nbFct);
 %Plot signal
-figure('position',[10 10 2300 500],'color',[1,1,1]); 
+figure('position',[10 10 1300 500],'color',[1,1,1]); 
 subplot(1,3,1:2); hold on; 
 for n=1:nbSamples
 	h(1) = plot(x(:,n), '-','lineWidth',6,'color',[.7 .7 .7]);
diff --git a/demos/demo_proMP_Fourier_sampling02.m b/demos/demo_proMP_Fourier_sampling02.m
index 80f42a4361467091c815c51d290e40eab449b3f1..f6981aa7cfe9618adb625131be15b740ec0a477c 100644
--- a/demos/demo_proMP_Fourier_sampling02.m
+++ b/demos/demo_proMP_Fourier_sampling02.m
@@ -39,6 +39,7 @@ nbVar = 2; %Dimension of position data (here: x1,x2)
 
 %% Load handwriting data
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+demos=[];
 load('data/2Dletters/B.mat');
 for n=1:nbSamples
 	dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData/2)); %Resampling
@@ -100,7 +101,7 @@ xr = Psi * xw;
 %% Plot 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %Plot signal
-figure('position',[10 10 2500 1000],'color',[1,1,1]); 
+figure('position',[10 10 1500 800],'color',[1,1,1]); 
 for i=1:nbVar
 	axLim = [1, nbData, min(real(Mu(i:nbVar:end)))-1, max(real(Mu(i:nbVar:end)))+1];
 	subplot(2,3,(i-1)*3+[1:2]); hold on;