diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m
index 97a35ebda855da9b40fad5918f447b184be9c02e..a25a8d9c0f65d179ea9c3e2cfcf040ecf699eaec 100644
--- a/demos/demo_ergodicControl_1D01.m
+++ b/demos/demo_ergodicControl_1D01.m
@@ -54,7 +54,7 @@ Sigma(:,2) = 0.01;
Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
-%% Compute Fourier series coefficients phi_k of desired spatial distribution
+%% Compute Fourier series coefficients w_hat of desired spatial distribution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
rg = [0:nbFct-1]';
Lambda = (rg.^2 + 1).^-1; %Weighting vector (Eq.(15)
@@ -62,40 +62,43 @@ Lambda = (rg.^2 + 1).^-1; %Weighting vector (Eq.(15)
HK = L; %Rescaling term (as scalar)
% HK = [1; sqrt(.5)*ones(nbFct-1,1)]; %Rescaling term (as normalizing vector)
-%Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
-w = rg .* om;
-phi = zeros(nbFct,1);
-for k=1:nbStates
- phi = phi + Priors(k) .* cos(w .* Mu(:,k)) .* exp(-.5 .* w.^2 .* Sigma(:,k)); %Eq.(21)
+%Explicit description of w_hat by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
+kk = rg .* om;
+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
-phi = phi ./ HK;
+w_hat = w_hat ./ HK;
%% Ergodic control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-ckt = zeros(nbFct, 1);
+wt = zeros(nbFct, 1);
for t=1:nbData
r.x(:,t) = x; %Log data
%Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction)
- fx = cos(x * rg .* om); %Eq.(18)
- gradf = -sin(x * rg .* om) .* rg .* om;
+ phi = cos(x * rg .* om); %Eq.(18)
+ dphi = -sin(x * rg .* om) .* rg .* om;
% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction)
% fx = real(exp(-i .* x * rg .* om)); %Eq.(18)
% gradf = real(-exp(-i .* x * rg .* om) .* i .* rg .* om);
- ckt = ckt + fx ./ HK; %ckt./t are the Fourier series coefficients along trajectory (Eq.(17))
+ wt = wt + phi ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
-% u = -gradf' * (Lambda .* (ckt./t - phi)) .* t .* 1E-1; %Velocity command
+% u = -gradf' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
%Controller with constrained velocity norm
- u = -gradf' * (Lambda .* (ckt./t - phi)); %Eq.(24)
+ u = -dphi' * (Lambda .* (wt./t - w_hat)); %Eq.(24)
u = u .* u_max ./ (norm(u)+1E-2); %Velocity command
x = x + u .* dt; %Update of position
end
+% % r.G = real(A * ck' / nbData);
+% invA = exp(-Xm' * KX *i*2*pi) / nbData ./ repmat(HK,nbRes,1);
+% r.G = real(invA * ck');
%% Plot
@@ -110,16 +113,15 @@ set(gca,'xtick',[],'ytick',[]);
%Plot distribution
subplot(1,3,3); hold on;
nbRes = 100;
-xm = linspace(xlim(1),xlim(2),nbRes);
-G = zeros(1,nbRes);
+x = linspace(xlim(1),xlim(2),nbRes);
+g = zeros(1,nbRes);
for k=1:nbStates
- G = G + Priors(k) .* mvnpdf(xm', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
+ g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
end
-plot(G, xm, '-','linewidth',3,'color',[.8 0 0]);
+plot(g, x, '-','linewidth',3,'color',[.8 0 0]);
xlabel('\phi(x)','fontsize',18); ylabel('x','fontsize',18);
set(gca,'xtick',[],'ytick',[]);
% print('-dpng','graphs/ergodicControl_1D01.png');
pause;
-close all;
-
+close all;
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index b1cd448f40b0348dee7da56efc8a591eb0d59074..f662e1496086b971e874a34581f5a28303025a90 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -72,40 +72,40 @@ HK = L^nbVar; %Rescaling term (as scalar)
op = hadamard(2^(nbVar-1));
op = op(1:nbVar,:);
%Compute phi_k
-w = KX(:,:) .* om;
-phi = zeros(nbFct^nbVar, 1);
-for k=1:nbStates
+kk = KX(:,:) .* om;
+w_hat = zeros(nbFct^nbVar, 1);
+for j=1:nbStates
for n=1:size(op,2)
- MuTmp = diag(op(:,n)) * Mu(:,k); %Eq.(20)
- SigmaTmp = diag(op(:,n)) * Sigma(:,:,k) * diag(op(:,n))'; %Eq.(20)
- phi = phi + Priors(k) .* cos(w'*MuTmp) .* exp(diag(-.5 * w' * SigmaTmp * w)); %Eq.(21)
+ MuTmp = diag(op(:,n)) * Mu(:,j); %Eq.(20)
+ SigmaTmp = diag(op(:,n)) * Sigma(:,:,j) * diag(op(:,n))'; %Eq.(20)
+ w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
end
end
-phi = phi ./ HK ./ size(op,2);
+w_hat = w_hat ./ HK ./ size(op,2);
%% Ergodic control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-ckt = zeros(nbFct^nbVar, 1);
+wt = zeros(nbFct^nbVar, 1);
for t=1:nbData
r.x(:,t) = x; %Log data
%Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction)
- fx = cos(x * rg .* om); %Eq.(18)
- dfx = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
+ phi1 = cos(x * rg .* om); %Eq.(18)
+ dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction)
-% fx = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% dfx = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
+% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
+% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
- gradf = [dfx(1,xx).*fx(2,yy); fx(1,xx).*dfx(2,yy)]; %gradient of basis functions
- ckt = ckt + (fx(1,xx) .* fx(2,yy))' ./ HK; %ckt./t are the Fourier series coefficients along trajectory (Eq.(17))
+ dphi = [dphi1(1,xx) .* phi1(2,yy); phi1(1,xx) .* dphi1(2,yy)]; %gradient of basis functions
+ wt = wt + (phi1(1,xx) .* phi1(2,yy))' ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
-% u = -gradf * (Lambda .* (ckt./t - phi)) .* t .* 5E-1; %Velocity command
+% u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 5E-1; %Velocity command
%Controller with constrained velocity norm
- u = -gradf * (Lambda .* (ckt./t - phi)); %Eq.(24)
+ u = -dphi * (Lambda .* (wt./t - w_hat)); %Eq.(24)
u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
x = x + u .* dt; %Update of position
@@ -122,21 +122,20 @@ axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal;
% print('-dpng','graphs/ergodicControl_2D01.png');
%Plot Fourier series coefficients
-figure('position',[1220,10,1300,700]);
+figure('position',[1220,10,1300,700]);
colormap(repmat(linspace(1,.4,64),3,1)');
-subplot(1,2,1); hold on; axis off; title('\phi_k','fontsize',20);
-imagesc(reshape(phi,nbFct,nbFct));
+subplot(1,2,1); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20);
+imagesc(reshape(w_hat,nbFct,nbFct));
axis tight; axis equal; axis ij;
-subplot(1,2,2); hold on; axis off; title('c_k','fontsize',20);
-imagesc(reshape(ckt./t,[nbFct,nbFct]));
+subplot(1,2,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20);
+imagesc(reshape(wt./t,[nbFct,nbFct]));
axis tight; axis equal; axis ij;
% %Plot Fourier series coefficients (alternative version)
% figure('position',[1220,10,1300,700]); hold on; axis off;
% colormap(repmat(linspace(1,.4,64),3,1)');
-% imagesc([phi, reshape(ckt./t,[nbFct,nbFct])]);
+% imagesc([w_hat, reshape(wt./t,[nbFct,nbFct])]);
% axis tight; axis equal; axis ij;
pause;
-close all;
-
+close all;
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_3D01.m b/demos/demo_ergodicControl_3D01.m
index 3c9d788b7ddc15ffd9a4d11b9c19b38302ac94b3..8865f5bbec8a5bbbc918c3662584007e90c2633c 100644
--- a/demos/demo_ergodicControl_3D01.m
+++ b/demos/demo_ergodicControl_3D01.m
@@ -72,40 +72,40 @@ HK = L^nbVar; %Rescaling term (as scalar)
op = hadamard(2^(nbVar-1));
op = op(1:nbVar,:);
%Compute phi_k
-w = KX(:,:) .* om;
-phi = zeros(nbFct^nbVar, 1);
-for k=1:nbStates
+kk = KX(:,:) .* om;
+w_hat = zeros(nbFct^nbVar, 1);
+for j=1:nbStates
for n=1:size(op,2)
- MuTmp = diag(op(:,n)) * Mu(:,k); %Eq.(20)
- SigmaTmp = diag(op(:,n)) * Sigma(:,:,k) * diag(op(:,n))'; %Eq.(20)
- phi = phi + Priors(k) .* cos(w'*MuTmp) .* exp(diag(-.5 * w' * SigmaTmp * w)); %Eq.(21)
+ MuTmp = diag(op(:,n)) * Mu(:,j); %Eq.(20)
+ SigmaTmp = diag(op(:,n)) * Sigma(:,:,j) * diag(op(:,n))'; %Eq.(20)
+ w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
end
end
-phi = phi ./ HK ./ size(op,2);
+w_hat = w_hat ./ HK ./ size(op,2);
%% Ergodic control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-ckt = zeros(nbFct^nbVar, 1);
+wt = zeros(nbFct^nbVar, 1);
for t=1:nbData
r.x(:,t) = x; %Log data
%Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction)
- fx = cos(x * rg .* om); %Eq.(18)
- dfx = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
+ phi1 = cos(x * rg .* om); %Eq.(18)
+ dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction)
-% fx = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% dfx = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
+% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
+% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
- gradf = [dfx(1,xx).*fx(2,yy).*fx(3,zz); fx(1,xx).*dfx(2,yy).*fx(3,zz); fx(1,xx).*fx(2,yy).*dfx(3,zz)]; %gradient of basis functions
- ckt = ckt + (fx(1,xx) .* fx(2,yy) .* fx(3,zz))' ./ HK; %ckt./t are the Fourier series coefficients along trajectory (Eq.(17))
+ dphi = [dphi1(1,xx) .* phi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* dphi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* phi1(2,yy) .* dphi1(3,zz)]; %gradient of basis functions
+ wt = wt + (phi1(1,xx) .* phi1(2,yy) .* phi1(3,zz))' ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
-% u = -gradf * (Lambda .* (ckt./t - phi)) .* t .* 1E-1; %Velocity command
+% u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
%Controller with constrained velocity norm
- u = -gradf * (Lambda .* (ckt./t - phi)); %Eq.(24)
+ u = -dphi * (Lambda .* (wt./t - w_hat)); %Eq.(24)
u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
x = x + u .* dt; %Update of position
@@ -124,5 +124,4 @@ axis([xlim(1),xlim(2),xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; axis vis3d;
% print('-dpng','graphs/ergodicControl_3D01.png');
pause;
-close all;
-
+close all;
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_nD01.m b/demos/demo_ergodicControl_nD01.m
index bcca83d6ebabb944145d77dc85f9dbbd0b19f6e9..5ee444689307cbf6ce93dc73b95a0fdddda77ec4 100644
--- a/demos/demo_ergodicControl_nD01.m
+++ b/demos/demo_ergodicControl_nD01.m
@@ -80,54 +80,54 @@ HK = L^nbVar; %Rescaling term (as scalar)
op = hadamard(2^(nbVar-1));
op = op(1:nbVar,:);
%Compute phi_k
-w = [];
+kk = [];
for n=1:nbVar
- w = [w; Karr(n).x(:)' .* om];
+ kk = [kk; Karr(n).x(:)' .* om];
end
-phi = zeros(nbFct^nbVar, 1);
-for k=1:nbStates
+w_hat = zeros(nbFct^nbVar, 1);
+for j=1:nbStates
for n=1:size(op,2)
- MuTmp = diag(op(:,n)) * Mu(:,k); %Eq.(20)
- SigmaTmp = diag(op(:,n)) * Sigma(:,:,k) * diag(op(:,n))'; %Eq.(20)
- phi = phi + Priors(k) .* cos(w'*MuTmp) .* exp(diag(-.5 * w' * SigmaTmp * w)); %Eq.(21)
+ MuTmp = diag(op(:,n)) * Mu(:,j); %Eq.(20)
+ SigmaTmp = diag(op(:,n)) * Sigma(:,:,j) * diag(op(:,n))'; %Eq.(20)
+ w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
end
end
-phi = phi ./ HK ./ size(op,2);
+w_hat = w_hat ./ HK ./ size(op,2);
%% Ergodic control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-ckt = zeros(nbFct^nbVar, 1);
+wt = zeros(nbFct^nbVar, 1);
for t=1:nbData
r.x(:,t) = x; %Log data
%Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction)
- fx = cos(x * rg .* om); %Eq.(18)
- dfx = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
+ phi1 = cos(x * rg .* om); %Eq.(18)
+ dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
% %Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction)
-% fx = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% dfx = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
+% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
+% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
- gradf = ones(nbVar, nbFct^nbVar);
- fxp = ones(nbFct^nbVar, 1);
+ dphi = ones(nbVar, nbFct^nbVar);
+ phi = ones(nbFct^nbVar, 1);
for n=1:nbVar
for m=1:nbVar
if m==n
- gradf(n,:) = gradf(n,:) .* dfx(m,arr(m).x(:));
+ dphi(n,:) = dphi(n,:) .* dphi1(m,arr(m).x(:));
else
- gradf(n,:) = gradf(n,:) .* fx(m,arr(m).x(:));
+ dphi(n,:) = dphi(n,:) .* phi1(m,arr(m).x(:));
end
end
- fxp = fxp .* fx(n,arr(n).x(:))';
+ phi = phi .* phi1(n,arr(n).x(:))';
end
- ckt = ckt + fxp ./ HK; %ckt./t are the Fourier series coefficients along trajectory (Eq.(17))
+ wt = wt + phi ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
-% u = -gradf * (Lambda .* (ckt./t - phi)) .* t .* 1E-1; %Velocity command
+% u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
%Controller with constrained velocity norm
- u = -gradf * (Lambda .* (ckt./t - phi)); %Eq.(24)
+ u = -dphi * (Lambda .* (wt./t - w_hat)); %Eq.(24)
u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
x = x + u .* dt; %Update of position
@@ -162,5 +162,4 @@ function arr = ndarr(lst, nbVar)
% x = cat(n+1,x,repmat(lst,s));
arr(n).x = repmat(lst,s);
end
-end
-
+end
\ No newline at end of file