Commit 65b56cf3 authored by Sylvain CALINON's avatar Sylvain CALINON

Ref updated

parent 694917d2
......@@ -277,10 +277,10 @@ If you find PbDlib useful for your research, please cite the related publication
</p>
<p><a name="ref-3">
[3] Calinon, S. and Jaquier, N. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
</a><br>
[[pdf]](http://calinon.ch/papers/Calinon-arXiv2019.pdf)
[[bib]](http://calinon.ch/papers/Calinon-arXiv2019.bib)
[[pdf]](http://calinon.ch/papers/Calinon-RAM2020.pdf)
[[bib]](http://calinon.ch/papers/Calinon-RAM2020.bib)
<br><strong>(Ref. for Riemannian manifolds)</strong>
</p>
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_Gdp_vecTransp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_Hd_GMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_Hd_interp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S1_interp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S1_interp02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR03
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR04
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GaussProd01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_TPGMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_TPGMM02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR03
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_batchLQR_Bezier01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_infHorLQR01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S2_vecTransp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMR01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMR02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S3_infHorLQR01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S3_interp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_S3_vecTransp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_SE2_GMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_SE2_interp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_SOd_interp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMM02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMR01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMR02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_Sd_GaussProd01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -6,7 +6,7 @@ function demo_Riemannian_Sd_MPC01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_MPC_infHor01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_interp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_interp02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_vecTransp01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_vecTransp02
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -3,7 +3,7 @@ function demo_Riemannian_pose_GMM01
%
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S. and Jaquier, N.",
% author="Calinon, S.",
% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
......
......@@ -36,7 +36,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 4000; %Number of datapoints
nbData = 8000; %Number of datapoints
nbFct = 50; %Number of basis functions along x and y
nbStates = 2; %Number of Gaussians to represent the spatial distribution
dt = 1E-2; %Time step
......@@ -44,7 +44,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim
L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
om = 2 .* pi ./ L; %omega
u_max = 1E0; %Maximum speed allowed
x = .1; %Initial position
%Desired spatial distribution represented as a mixture of Gaussians
Mu(:,1) = 0.7 *1;
......@@ -59,69 +58,161 @@ Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
rg = [0:nbFct-1]';
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 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
w_hat = w_hat ./ HK;
w_hat = w_hat ./ L;
% %Alternative computation of w_hat by discretization (for verification)
% nbRes = 1000;
% x = linspace(xlim(1), xlim(2), nbRes); %Spatial range
% g = zeros(1,nbRes);
% for k=1:nbStates
% g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
% end
% phi_inv = cos(rg * x .* om) ./ L ./ nbRes;
% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
%
% % w_hat = zeros(nbFct,1);
% % for n=1:nbFct
% % w_hat(n) = sum(g .* cos(x .* rg(n) .* om));
% % end
% % w_hat = w_hat ./ L ./ nbRes; %Fourier coefficients of spatial distribution
%Fourier basis functions (for a discretized map)
nbRes = 100;
xm = linspace(xlim(1), xlim(2), nbRes); %Spatial range
phim = cos(rg * xm .* om) .* 2; %Fourier basis functions
phim(2:end,:) = phim(2:end,:) .* 2;
%Desired spatial distribution
g = w_hat' * phim;
% %Alternative computation of g
% g = zeros(1,nbRes);
% for k=1:nbStates
% g = g + Priors(k) .* mvnpdf(xm', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
% %(2.*pi.*s1).^-.5 .* exp(-.5 .* s1.^-1 .* (X - m1).^2);
% end
%% Ergodic control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = .1; %Initial position
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)
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);
dphi = -sin(x * rg .* om) .* rg .* om; %Gradient of basis functions
wt = wt + phi ./ HK; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
wt = wt + phi ./ L; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
% u = -gradf' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
% u = -dphi' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
%Controller with constrained velocity norm
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
r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution
% r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
end
% % r.G = real(A * ck' / nbData);
% invA = exp(-Xm' * KX *i*2*pi) / nbData ./ repmat(HK,nbRes,1);
% r.G = real(invA * ck');
% %Alternative computation of desired and reconstructed spatial distribution
% g = zeros(1,nbRes);
% for n=1:nbFct
% g = g + w_hat(n) .* cos(xm .* rg(n) .* om);
% end
% r.g = zeros(1,nbRes);
% for n=1:nbFct
% r.g = r.g + (wt(n)./nbData) .* cos(xm .* rg(n) .* om);
% end
%% Plot
%% Plot (static)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,1800,900],'color',[1 1 1]);
%Plot signal
subplot(1,3,[1,2]); hold on;
plot(1:nbData, r.x, '-','linewidth',3,'color',[.2 .2 .2]);
axis([1, nbData, xlim']);
xlabel('t','fontsize',18); ylabel('x','fontsize',18);
set(gca,'xtick',[],'ytick',[]);
s = nbData; %Reconstruction at time step s
T = nbData; %Size of time window
figure('position',[10,10,1200,1200],'color',[1 1 1]);
%Plot distribution
subplot(1,3,3); hold on;
nbRes = 100;
x = linspace(xlim(1),xlim(2),nbRes);
g = zeros(1,nbRes);
for k=1:nbStates
g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
end
plot(g, x, '-','linewidth',3,'color',[.8 0 0]);
xlabel('\phi(x)','fontsize',18); ylabel('x','fontsize',18);
set(gca,'xtick',[],'ytick',[]);
subplot(3,1,1); hold on;
h(1) = plot(xm, g, '-','linewidth',3,'color',[.8 0 0]);
h(2) = plot(xm, r.g(:,s), '-','linewidth',3,'color',[0 0 0]);
legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest');
axis([xlim', -.3, max(g)*1.2]);
xlabel('$x$','interpreter','latex','fontsize',28);
ylabel('$g(x)$','interpreter','latex','fontsize',28);
set(gca,'linewidth',2,'xtick',[],'ytick',[]);
%Plot signal
subplot(3,1,[2,3]); hold on; box on;
plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]);
plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0]);
axis([xlim', 1, T]);
xlabel('$x$','interpreter','latex','fontsize',28);
set(gca,'linewidth',2,'xtick',[],'ytick',[1,T],'yticklabel',{'t-T','t'},'fontsize',24);
% print('-dpng','graphs/ergodicControl_1D01.png');
% %Plot error
% figure; hold on;
% plot(r.e(200:end));
% %Plot decomposition of desired distribution
% dg = 4;
% figure('position',[10,10,900,1200],'color',[1 1 1]); hold on; axis off;
% %Plot distribution
% plot(xm, g*2, '-','linewidth',3,'color',[.8 0 0]);
% for n=1:6
% plot(xm, .3*phim(n,:)-dg*n,'-','linewidth',2,'color',[0 .6 0]);
% end
% % print('-dpng','graphs/ergodicControl_1DbasisFcts01.png');
% %% Plot (animated)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% T = 2000; %Size of time window
% s0 = 1; %Animation from time step s0
% figure('position',[10,10,1200,1200],'color',[1 1 1]);
% %Plot distribution
% subplot(3,1,1); hold on;
% h(1) = plot(xm, g, '-','linewidth',3,'color',[.8 0 0]);
% h(2) = plot(xm, r.g(:,s0), '-','linewidth',3,'color',[0 0 0]);
% axis([xlim', -.3, max(g)*1.2]);
% xlabel('$x$','interpreter','latex','fontsize',28);
% ylabel('$g(x)$','interpreter','latex','fontsize',28);
% set(gca,'linewidth',2,'xtick',[],'ytick',[]);
% %Plot signal
% subplot(3,1,[2,3]); hold on; box on;
% axis([xlim', 1, T]);
% xlabel('$x$','interpreter','latex','fontsize',28);
% set(gca,'linewidth',2,'xtick',[],'ytick',[1,T],'yticklabel',{'t-T','t'},'fontsize',24);
% %Animation
% hs=[]; id=1;
% for s = s0:500:nbData
% %Plot distribution
% subplot(3,1,1); hold on;
% delete(h(2));
% h(2) = plot(xm, r.g(:,s), '-','linewidth',3,'color',[0 0 0]);
% legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest');
% %Plot signal
% subplot(3,1,[2,3]); hold on; box on;
% delete(hs);
% if s>T
% hs(1) = plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]);
% else
% hs(1) = plot(r.x(:,1:s), T-s+1:T, '-','linewidth',3,'color',[0 0 0]);
% end
% hs(2) = plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0]);
% drawnow;
% % print('-dpng',['graphs/anim/ergodicControl_1Danim' num2str(id,'%.3d') '.png']);
% % id = id + 1;
% % pause(.1);
% end
pause;
close all;
\ No newline at end of file
......@@ -36,7 +36,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 4000; %Number of datapoints
nbData = 8000; %Number of datapoints
nbFct = 20; %Number of basis functions along x and y
nbVar = 2; %Dimension of datapoint
nbStates = 2; %Number of Gaussians to represent the spatial distribution
......@@ -45,8 +45,7 @@ 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
u_max = 5E0; %Maximum speed allowed
x = [.1; .3]; %Initial position
u_max = 5E1; %Maximum speed allowed
%Desired spatial distribution represented as a mixture of Gaussians
Mu(:,1) = [.3; .7];
......@@ -63,10 +62,6 @@ rg = 0:nbFct-1;
[KX(1,:,:), KX(2,:,:)] = ndgrid(rg, rg);
Lambda = (KX(1,:).^2 + KX(2,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15))
HK = L^nbVar; %Rescaling term (as scalar)
% hk = [1; sqrt(.5)*ones(nbFct-1,1)];
% HK = hk(xx,1) .* hk(yy,1); %Rescaling term (as normalizing matrix)
%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,11 +76,46 @@ 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 ./ HK ./ size(op,2);
w_hat = w_hat ./ L^nbVar ./ size(op,2);
% %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
% end
% 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 = 40;
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
% % phim(2:end,:) = phim(2:end,:) .* 2;
% phim = phim .* 2^nbVar;
% phim(1:nbFct,:) = phim(1:nbFct,:) .* .5;
% phim(1:nbFct:end,:) = phim(1:nbFct:end,:) .* .5;
hk = [1; 2*ones(nbFct-1,1)];
HK = hk(xx(:)) .* hk(yy(:));
phim = phim .* repmat(HK,[1,nbRes^nbVar]);
%Desired spatial distribution
g = w_hat' * phim;
% %Alternative computation of g
% g = zeros(1,nbRes^nbVar);
% for k=1:nbStates
% g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
% end
%% Ergodic control
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = [.1; .3]; %Initial position
wt = zeros(nbFct^nbVar, 1);
for t=1:nbData
r.x(:,t) = x; %Log data
......@@ -94,12 +124,8 @@ for t=1:nbData
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)
% phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
% dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
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))
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))' ./ L.^nbVar; %wt./t are the Fourier series coefficients along trajectory (Eq.(17))
% %Controller with ridge regression formulation
% u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 5E-1; %Velocity command
......@@ -109,33 +135,69 @@ for t=1:nbData
u = u .* u_max ./ (norm(u)+1E-1); %Velocity command