From adbda65cc1c07b1066fe2c69ee1655362e2162b9 Mon Sep 17 00:00:00 2001
From: scalinon <sylvain.calinon@idiap.ch>
Date: Mon, 30 Mar 2020 10:49:51 +0200
Subject: [PATCH] Publication reference updated
---
README.md | 2 +-
demos/demo_MPC03.m | 64 +++---
demos/demo_MPC04.m | 10 +-
demos/demo_MPC_MP01.m | 74 +++----
demos/demo_MPC_fullQ03.m | 63 +++---
demos/demo_MPC_iterativeLQR01.m | 8 +-
demos/demo_MPC_iterativeLQR_augmSigma01.m | 3 +-
demos/demo_MPC_robot01.m | 2 +-
demos/demo_Riemannian_Gdp_vecTransp01.m | 2 +-
demos/demo_Riemannian_Hd_GMM01.m | 2 +-
demos/demo_Riemannian_Hd_interp01.m | 2 +-
demos/demo_Riemannian_S1_interp01.m | 2 +-
demos/demo_Riemannian_S1_interp02.m | 2 +-
demos/demo_Riemannian_S2_GMM01.m | 2 +-
demos/demo_Riemannian_S2_GMR01.m | 2 +-
demos/demo_Riemannian_S2_GMR02.m | 2 +-
demos/demo_Riemannian_S2_GMR03.m | 2 +-
demos/demo_Riemannian_S2_GMR04.m | 2 +-
demos/demo_Riemannian_S2_GaussProd01.m | 2 +-
demos/demo_Riemannian_S2_TPGMM01.m | 2 +-
demos/demo_Riemannian_S2_TPGMM02.m | 2 +-
demos/demo_Riemannian_S2_batchLQR01.m | 2 +-
demos/demo_Riemannian_S2_batchLQR02.m | 2 +-
demos/demo_Riemannian_S2_batchLQR03.m | 2 +-
demos/demo_Riemannian_S2_batchLQR_Bezier01.m | 2 +-
demos/demo_Riemannian_S2_infHorLQR01.m | 2 +-
demos/demo_Riemannian_S2_vecTransp01.m | 2 +-
demos/demo_Riemannian_S3_GMM01.m | 2 +-
demos/demo_Riemannian_S3_GMR01.m | 2 +-
demos/demo_Riemannian_S3_GMR02.m | 2 +-
demos/demo_Riemannian_S3_infHorLQR01.m | 2 +-
demos/demo_Riemannian_S3_interp01.m | 2 +-
demos/demo_Riemannian_S3_vecTransp01.m | 2 +-
demos/demo_Riemannian_SE2_GMM01.m | 2 +-
demos/demo_Riemannian_SE2_interp01.m | 2 +-
demos/demo_Riemannian_SOd_interp01.m | 2 +-
demos/demo_Riemannian_Sd_GMM01.m | 2 +-
demos/demo_Riemannian_Sd_GMM02.m | 2 +-
demos/demo_Riemannian_Sd_GMR01.m | 2 +-
demos/demo_Riemannian_Sd_GMR02.m | 2 +-
demos/demo_Riemannian_Sd_GaussProd01.m | 2 +-
demos/demo_Riemannian_Sd_MPC01.m | 2 +-
demos/demo_Riemannian_Sd_MPC_infHor01.m | 2 +-
demos/demo_Riemannian_Sd_interp01.m | 2 +-
demos/demo_Riemannian_Sd_interp02.m | 2 +-
demos/demo_Riemannian_Sd_vecTransp01.m | 2 +-
demos/demo_Riemannian_Sd_vecTransp02.m | 2 +-
demos/demo_Riemannian_pose_GMM01.m | 2 +-
demos/demo_ergodicControl_1D01.m | 200 +++++++++++++------
demos/demo_ergodicControl_2D01.m | 143 ++++++++-----
demos/demo_ergodicControl_3D01.m | 4 +-
demos/demo_proMP01.m | 4 +-
52 files changed, 390 insertions(+), 267 deletions(-)
diff --git a/README.md b/README.md
index 5641eef..36144ec 100644
--- a/README.md
+++ b/README.md
@@ -277,7 +277,7 @@ If you find PbDlib useful for your research, please cite the related publication
</p>
<p><a name="ref-3">
-[3] Calinon, S. (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: Applications for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
</a><br>
[[pdf]](http://calinon.ch/papers/Calinon-RAM2020.pdf)
[[bib]](http://calinon.ch/papers/Calinon-RAM2020.bib)
diff --git a/demos/demo_MPC03.m b/demos/demo_MPC03.m
index bdbe439..d03c75c 100644
--- a/demos/demo_MPC03.m
+++ b/demos/demo_MPC03.m
@@ -1,5 +1,5 @@
function demo_MPC03
-% Batch computation of linear quadratic tracking problem, by tracking position and velocity references.
+% Batch computation of linear quadratic tracking problem, by tracking only position references.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
@@ -42,10 +42,10 @@ nbData = 200; %Number of datapoints
model.nbStates = 8; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
-model.nbDeriv = 1; %Number of static & dynamic features (D=1 for just x)
+model.nbDeriv = 1; %Number of static & dynamic features (nbDeriv=1 for just x)
model.nbVar = model.nbVarPos * model.nbDeriv; %Dimension of state vector
-model.rfactor = 1E-6; %Control cost in LQR
model.dt = 0.01; %Time step duration
+model.rfactor = 1E-3; %Control cost in LQR
%Control cost matrix
R = eye(model.nbVarPos) * model.rfactor;
@@ -54,27 +54,25 @@ R = kron(eye(nbData-1),R);
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbDeriv = model.nbDeriv + 1; %For definition of dynamical system
-
% %Integration with Euler method
-% Ac1d = diag(ones(nbDeriv-1,1),1);
-% Bc1d = [zeros(nbDeriv-1,1); 1];
-% A = kron(eye(nbDeriv)+Ac1d*model.dt, eye(model.nbVarPos));
+% Ac1d = diag(ones(model.nbDeriv-1,1),1);
+% Bc1d = [zeros(model.nbDeriv-1,1); 1];
+% A = kron(eye(model.nbDeriv)+Ac1d*model.dt, eye(model.nbVarPos));
% B = kron(Bc1d*model.dt, eye(model.nbVarPos));
% C = kron([1, 0], eye(model.nbVarPos));
%Integration with higher order Taylor series expansion
-A1d = zeros(nbDeriv);
-for i=0:nbDeriv-1
- A1d = A1d + diag(ones(nbDeriv-i,1),i) * model.dt^i * 1/factorial(i); %Discrete 1D
+A1d = zeros(model.nbDeriv);
+for i=0:model.nbDeriv-1
+ A1d = A1d + diag(ones(model.nbDeriv-i,1),i) .* model.dt^i ./ factorial(i); %Discrete 1D
end
-B1d = zeros(nbDeriv,1);
-for i=1:nbDeriv
- B1d(nbDeriv-i+1) = model.dt^i * 1/factorial(i); %Discrete 1D
+B1d = zeros(model.nbDeriv,1);
+for i=1:model.nbDeriv
+ B1d(model.nbDeriv-i+1) = model.dt^i ./ factorial(i); %Discrete 1D
end
A = kron(A1d, eye(model.nbVarPos)); %Discrete nD
B = kron(B1d, eye(model.nbVarPos)); %Discrete nD
-C = kron([1, 0], eye(model.nbVarPos));
+C = kron([1, zeros(1,model.nbDeriv-1)], eye(model.nbVarPos));
% %Conversion with control toolbox
% Ac1d = diag(ones(nbDeriv-1,1),1); %Continuous 1D
@@ -86,9 +84,9 @@ C = kron([1, 0], eye(model.nbVarPos));
% C = kron([1, 0], eye(model.nbVarPos));
-%Build CSx and CSu matrices for batch LQR, see Eq. (35)
+%Build CSx and CSu matrices for batch LQR
CSu = zeros(model.nbVarPos*nbData, model.nbVarPos*(nbData-1));
-CSx = kron(ones(nbData,1), [eye(model.nbVarPos) zeros(model.nbVarPos)]);
+CSx = kron(ones(nbData,1), [eye(model.nbVarPos) zeros(model.nbVarPos, model.nbVarPos*(model.nbDeriv-1))]);
M = B;
for n=2:nbData
id1 = (n-1)*model.nbVarPos+1:n*model.nbVarPos;
@@ -105,15 +103,7 @@ end
load('data/2Dletters/E.mat');
Data=[];
for n=1:nbSamples
- s(n).Data=[];
- for m=1:model.nbDeriv
- if m==1
- dTmp = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
- else
- dTmp = gradient(dTmp) / model.dt; %Compute derivatives
- end
- s(n).Data = [s(n).Data; dTmp];
- end
+ s(n).Data = spline(1:size(demos{n}.pos,2), demos{n}.pos, linspace(1,size(demos{n}.pos,2),nbData)); %Resampling
Data = [Data s(n).Data];
end
@@ -121,8 +111,8 @@ end
%% Learning
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('Parameters estimation...');
-%model = init_GMM_kmeans(Data,model);
-model = init_GMM_kbins(Data,model,nbSamples);
+%model = init_GMM_kmeans(Data, model);
+model = init_GMM_kbins(Data, model, nbSamples);
%Refinement of parameters
[model, H] = EM_GMM(Data, model);
@@ -148,11 +138,11 @@ CSuInvSigmaQ = CSu' / SigmaQ;
Rq = CSuInvSigmaQ * CSu + R;
%Reproductions
for n=1:nbRepros
- %X = [Data(:,1)+randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)];
- X = [Data(:,1); zeros(model.nbVarPos,1)];
- rq = CSuInvSigmaQ * (MuQ-CSx*X);
+ %x = [Data(:,1)+randn(model.nbVarPos,1)*2E0; zeros(model.nbVarPos,1)];
+ x = [Data(:,1); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
+ rq = CSuInvSigmaQ * (MuQ-CSx*x);
u = Rq \ rq; %Can also be computed with u = lscov(Rq, rq);
- r(n).Data = reshape(CSx*X+CSu*u, model.nbVarPos, nbData);
+ r(n).Data = reshape(CSx*x+CSu*u, model.nbVarPos, nbData);
end
@@ -200,20 +190,20 @@ end
%% Stochastic sampling by exploiting distribution on x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbEigs = 2; %Number of principal eigencomponents to keep
-X = [Data(:,1); zeros(model.nbVarPos,1)];
+x = [Data(:,1); zeros(model.nbVarPos*(model.nbDeriv-1),1)];
CSuInvSigmaQ = CSu' / SigmaQ;
Rq = CSuInvSigmaQ * CSu + R;
-rq = CSuInvSigmaQ * (MuQ-CSx*X);
+rq = CSuInvSigmaQ * (MuQ-CSx*x);
u = Rq \ rq;
-Mu = CSx * X + CSu * u;
+Mu = CSx * x + CSu * u;
mse = 1; %abs(MuQ'/SigmaQ*MuQ - rq'/Rq*rq) ./ (model.nbVarPos*nbData);
-Sigma = CSu/Rq*CSu' * mse + eye(nbData*model.nbVar) * 0E-10;
+Sigma = CSu/Rq*CSu' * mse + eye(nbData*model.nbVarPos) * 0E-10;
%[V,D] = eigs(Sigma);
[V,D] = eig(Sigma);
for n=1:nbNewRepros
%xtmp = Mu + V(:,1:nbEigs) * D(1:nbEigs,1:nbEigs).^.5 * randn(nbEigs,1) * 0.1;
xtmp = Mu + V * D.^.5 * randn(size(D,1),1) * 0.9;
- rnew(n).Data = reshape(xtmp, model.nbVar, nbData); %Reshape data for plotting
+ rnew(n).Data = reshape(xtmp, model.nbVarPos, nbData); %Reshape data for plotting
end
diff --git a/demos/demo_MPC04.m b/demos/demo_MPC04.m
index b3444e6..3d28f9a 100644
--- a/demos/demo_MPC04.m
+++ b/demos/demo_MPC04.m
@@ -35,7 +35,7 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-model.nbStates = 4; %Number of Gaussians in the GMM
+model.nbStates = 6; %Number of Gaussians in the GMM
model.nbVarPos = 2; %Dimension of position data (here: x1,x2)
model.nbDeriv = 2; %Number of static & dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
model.nbVar = model.nbVarPos * (model.nbDeriv+1); %Dimension of augmented state vector
@@ -135,11 +135,11 @@ Q(1:12,1:12)
SuInvSigmaQ = Su' * Q;
Rq = SuInvSigmaQ * Su + R;
for n=1:nbRepros
- X = Data(:,1);
- %X = [Data(1:4,1); model.Mu(5:6,end)];
- rq = SuInvSigmaQ * (MuQ-Sx*X);
+ x = Data(:,1);
+ %x = [Data(1:4,1); model.Mu(5:6,end)];
+ rq = SuInvSigmaQ * (MuQ-Sx*x);
u = Rq \ rq;
- r(n).Data = reshape(Sx*X+Su*u, model.nbVar, nbData);
+ r(n).Data = reshape(Sx*x+Su*u, model.nbVar, nbData);
r(n).u = reshape(u, model.nbVarPos, nbData-1);
end
diff --git a/demos/demo_MPC_MP01.m b/demos/demo_MPC_MP01.m
index cbb8760..6451406 100644
--- a/demos/demo_MPC_MP01.m
+++ b/demos/demo_MPC_MP01.m
@@ -36,48 +36,31 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbVarPos = 2; %Dimension of position data (here: x1,x2)
-nbDeriv = 2; %Number of static & dynamic features (D=2 for [x,dx])
-nbVar = nbVarPos * nbDeriv; %Dimension of state vector
+nbDeriv = 1; %Number of static & dynamic features (nbDeriv=2 for [x,dx])
nbData = 200; %Number of datapoints in a trajectory
dt = 0.01; %Time step duration
-rfactor = 1E2; %Control cost in LQR
+rfactor = 1E2; %Control cost in LQR
R = speye((nbData-1)*nbVarPos) * rfactor;
%% Dynamical System settings (discrete version)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Integration with higher order Taylor series expansion
A1d = zeros(nbDeriv);
for i=0:nbDeriv-1
- A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D
+ A1d = A1d + diag(ones(nbDeriv-i,1),i) .* dt^i ./ factorial(i); %Discrete 1D
end
B1d = zeros(nbDeriv,1);
for i=1:nbDeriv
- B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D
+ B1d(nbDeriv-i+1) = dt^i ./ factorial(i); %Discrete 1D
end
-A = kron(A1d, speye(nbVarPos)); %Discrete nD
-B = kron(B1d, speye(nbVarPos)); %Discrete nD
-C = kron([1, 0], eye(nbVarPos));
-
-% %Construct Su and Sx matrices (transfer matrices in batch LQR)
-% Su = zeros(nbVar*nbData, nbVarPos*(nbData-1));
-% Sx = kron(ones(nbData,1),eye(nbVar));
-% M = B;
-% for n=2:nbData
-% %Build Sx matrix
-% id1 = (n-1)*nbVar+1:nbData*nbVar;
-% Sx(id1,:) = Sx(id1,:) * A;
-% %Build Su matrix
-% id1 = (n-1)*nbVar+1:n*nbVar;
-% id2 = 1:(n-1)*nbVarPos;
-% Su(id1,id2) = M;
-% M = [A*M(:,1:nbVarPos), M];
-% end
+A = kron(A1d, eye(nbVarPos)); %Discrete nD
+B = kron(B1d, eye(nbVarPos)); %Discrete nD
+C = kron([1, zeros(1,nbDeriv-1)], eye(nbVarPos));
-%Construct CSu and CSx matrices (transfer matrices in batch LQR)
+%Build CSx and CSu matrices for batch LQR
CSu = zeros(nbVarPos*nbData, nbVarPos*(nbData-1));
-CSx = kron(ones(nbData,1), [eye(nbVarPos) zeros(nbVarPos)]);
+CSx = kron(ones(nbData,1), [eye(nbVarPos) zeros(nbVarPos, nbVarPos*(nbDeriv-1))]);
M = B;
for n=2:nbData
id1 = (n-1)*nbVarPos+1:n*nbVarPos;
@@ -117,7 +100,7 @@ end
Psi = kron(phi, eye(nbVarPos));
-% w = (Psi' * Psi + eye(nbFct).*1E-18) \ Psi' * x;
+% w = (Psi' * Psi + eye(nbFct*nbVarPos).*1E-18) \ Psi' * x;
w = pinv(Psi) * x;
%Distribution in parameter space
@@ -132,12 +115,28 @@ Q = Psi / Sigma_w * Psi'; %Precision matrix
%% Reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-x0 = [x(1:2,1)+.2; zeros(nbVarPos,1)];
-% u = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx*x0));
-% rx = reshape(Sx*x0+Su*u, nbVarPos, nbData);
-u = (CSu' * Q * CSu + R) \ (CSu' * Q * (MuQ - CSx*x0));
-rx = reshape(CSx*x0+CSu*u, nbVarPos, nbData);
-
+x0 = [x(1:2,1)+.2; zeros(nbVarPos*(nbDeriv-1),1)];
+
+% % u = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - Sx*x0));
+% % rx = reshape(Sx*x0+Su*u, nbVarPos, nbData);
+% u = (CSu' * Q * CSu + R) \ (CSu' * Q * (MuQ - CSx*x0));
+% rx = reshape(CSx*x0+CSu*u, nbVarPos, nbData);
+
+%Alternative computation highlighting the linear relation to Mu_w and x0 in the solution
+A0 = (CSu' * Q * CSu + R) \ CSu' * Q;
+A1 = CSu * A0 * Psi;
+A2 = CSx - CSu * A0 * CSx;
+rx = reshape(A1 * Mu_w + A2 * x0, nbVarPos, nbData);
+
+%Comparison with Gaussian conditioning
+in = 1:nbVarPos;
+out = nbVarPos+1:nbVarPos*nbData;
+Sigma = Psi * Sigma_w * Psi';
+rx_out = MuQ(out) + Sigma(out,in) / Sigma(in,in) * (x0(1:nbVarPos) - MuQ(in));
+% rx_out = MuQ(out) - Q(out,out) \ Q(out,in) * (x0(1:nbVarPos) - MuQ(in));
+rx2 = reshape([x0(1:nbVarPos); rx_out], nbVarPos, nbData);
+
+% %Computation of covariances
% uSigma = inv(Rq) .* 1E-1;
% xSigma = Su * uSigma * Su';
@@ -150,8 +149,9 @@ for n=1:nbSamples
plot(x(1:2:end,n), x(2:2:end,n), '-','linewidth',4,'color',[.7,.7,.7]);
end
plot(MuQ(1:2:end), MuQ(2:2:end), '-','linewidth',4,'color',[.8 0 0]);
-plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0,0,0]);
plot(rx(1,1), rx(2,1), '.','markersize',28,'color',[0,0,0]);
+plot(rx(1,:), rx(2,:), '-','linewidth',4,'color',[0,0,0]);
+plot(rx2(1,:), rx2(2,:), '-','linewidth',4,'color',[0,0,.8]);
axis equal;
%Timeline Plots
@@ -180,6 +180,12 @@ end
set(gca,'xtick',[],'ytick',[]);
xlabel('$t$','interpreter','latex','fontsize',34); ylabel('$\phi_k$','interpreter','latex','fontsize',34);
+% %Psi*Psi' plot
+% figure; hold on; axis off; title('\Psi\Psi^T','fontsize',16);
+% colormap(flipud(gray));
+% imagesc(abs(Psi * Psi'));
+% axis tight; axis square; axis ij;
+
% print('-dpng','graphs/MPC_MP01.png');
pause;
close all;
\ No newline at end of file
diff --git a/demos/demo_MPC_fullQ03.m b/demos/demo_MPC_fullQ03.m
index 5f25b0b..7b12623 100644
--- a/demos/demo_MPC_fullQ03.m
+++ b/demos/demo_MPC_fullQ03.m
@@ -1,5 +1,5 @@
function demo_MPC_fullQ03
-% Batch LQT exploiting full Q matrix to constrain the motion of two agents in a ballistic task.
+% Batch LQT problem exploiting full Q matrix to constrain the motion of two agents in a simple ballistic task.
%
% If this code is useful for your research, please cite the related publication:
% @incollection{Calinon19chapter,
@@ -37,7 +37,7 @@ addpath('./m_fcts/');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbData = 200; %Number of datapoints
nbAgents = 2; %Number of agents
-nbVarPos = 2 * nbAgents; %Dimension of position data (here: x1,x2)
+nbVarPos = 2 * nbAgents; %Dimension of position data (here: x1,x2 for the two agents)
nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx)
nbVar = nbVarPos * (nbDeriv+1); %Dimension of state vector
dt = 1E-2; %Time step duration
@@ -51,7 +51,7 @@ R = speye((nbData-1)*nbVarPos) * rfactor; %Control cost matrix
% Ad0 = speye(6) + Ac0 * dt;
% Ad0 = kron(Ad0, speye(nbAgents)); %Discrete nD for several agents
-Ac1 = kron([0, 1, 0; 0, 0, 5; 0, 0, 0], speye(2));
+Ac1 = kron([0, 1, 0; 0, 0, 1; 0, 0, 0], speye(2));
Ad1 = speye(6) + Ac1 * dt;
Bc1 = kron([0; 1; 0], speye(2));
@@ -70,8 +70,8 @@ Bd = kron(Bd1, speye(nbAgents)); %Discrete nD for several agents
% return
%Build Sx and Su transfer matrices(for heterogeneous A and B)
-A = zeros(nbVar,nbVar,nbData);
-B = zeros(nbVar,nbVarPos,nbData);
+A = zeros(nbVar,nbVar,nbData-1);
+B = zeros(nbVar,nbVarPos,nbData-1);
for t=1:nbData
A(:,:,t) = Ad;
% A(5:8,9:12,t) = 0; %no gravity on agents
@@ -80,10 +80,10 @@ for t=1:nbData
if t==1
% A(:,:,t) = Ad0;
% B(:,:,t) = Bd;
- B(:,1:2,t) = Bd(:,1:2);
+ B(:,1:2,t) = Bd(:,1:2); %Agent 1 is only allowed to apply control commands at first time step
end
if t==1
- B(:,3:4,t) = Bd(:,3:4);
+ B(:,3:4,t) = Bd(:,3:4); %Agent 2 is only allowed to apply control commands at first time step
end
end
[Su, Sx] = transferMatrices(A, B);
@@ -93,19 +93,21 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MuQ = zeros(nbVar*nbData,1);
Q = zeros(nbVar*nbData);
-id = [1:nbVarPos] + (nbData-1) * nbVar;
+
+%Constraining the two agents to meet at tEvent
+tEvent = (nbData-1);
+id = [1:nbVarPos] + tEvent * nbVar;
Q(id,id) = eye(nbVarPos);
MuQ(id) = rand(nbVarPos,1);
-%Constraining the two agents to meet at the end of the motion
-MuQ(id(3:4)) = MuQ(id(1:2)); %Proposed meeting point for the two agents
+MuQ(id(3:4)) = MuQ(id(1:2)); %Proposed meeting point for the two agents (does not need to be this point)
Q(id(1:2), id(3:4)) = -eye(2)*1E0;
Q(id(3:4), id(1:2)) = -eye(2)*1E0;
-%% Batch LQR reproduction
+%% Batch LQT reproduction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-x0 = [rand(4,1); zeros(4,1); 0; -9.81*1E-2; 0; -9.81*0E-2];
+x0 = [rand(4,1); zeros(4,1); 0; -9.81*1E-2; 0; -9.81*1E-2]; %Both agents have random initial positions and are affected by gravity
u = (Su' * Q * Su + R) \ Su' * Q * (MuQ - Sx * x0);
rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
@@ -116,21 +118,27 @@ rx = reshape(Sx*x0+Su*u, nbVar, nbData); %Reshape data for plotting
%% Plot 2D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10 10 1000 1000],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
+
%Agent 1
% for t=1:nbData
% plotGMM(rx(1:2,t), xSigma(nbVar*(t-1)+[1,2],nbVar*(t-1)+[1,2]).*1E-8, [.2 .2 .2], .03); %Plot uncertainty
% end
-plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
-plot(rx(1,1), rx(2,1), '.','markersize',35,'color',[0 0 0]);
+h(1) = plot(rx(1,:), rx(2,:), '-','linewidth',2,'color',[0 0 0]);
+plot(rx(1,1), rx(2,1), '.','markersize',25,'color',[0 0 0]);
+plot2DArrow(rx(1:2,1), u(1:2).*1E-2, [.8 0 0]);
% plot(rx(1,end-3), rx(2,end-2), '.','markersize',25,'color',[0 .6 0]); %meeting point
% plot(MuQ(id(1)), MuQ(id(2)), '.','markersize',35,'color',[.8 0 0]);
+
%Agent 2
% for t=1:nbData
% plotGMM(rx(3:4,t), xSigma(nbVar*(t-1)+[3,4],nbVar*(t-1)+[3,4]).*1E-8, [.2 .2 .2], .03); %Plot uncertainty
% end
-plot(rx(3,:), rx(4,:), '-','linewidth',2,'color',[.6 .6 .6]);
-plot(rx(3,1), rx(4,1), '.','markersize',35,'color',[.6 .6 .6]);
-plot(rx(3,end-1), rx(4,end), '.','markersize',35,'color',[.4 .8 .4]); %meeting point
+h(2) = plot(rx(3,:), rx(4,:), '-','linewidth',2,'color',[.6 .6 .6]);
+plot(rx(3,1), rx(4,1), '.','markersize',25,'color',[.6 .6 .6]);
+plot2DArrow(rx(3:4,1), u(3:4).*1E-2, [.8 0 0]);
+h(3) = plot(rx(3,tEvent+1), rx(4,tEvent+1), '.','markersize',25,'color',[.4 .8 .4]); %meeting point
+
+legend(h,{'Agent 1','Agent 2','End point'});
% plot(MuQ(id(3)), MuQ(id(4)), '.','markersize',35,'color',[1 .6 .6]);
axis equal;
% print('-dpng','graphs/MPC_fullQ03a.png');
@@ -150,18 +158,15 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%
function [Su, Sx] = transferMatrices(A, B)
[nbVarX, nbVarU, nbData] = size(B);
- Su = zeros(nbVarX*nbData, nbVarU*(nbData-1));
+ nbData = nbData+1;
Sx = kron(ones(nbData,1), speye(nbVarX));
- M = speye(nbVarX);
- N = [];
- for t=2:nbData
- id1 = (t-1)*nbVarX+1:nbData*nbVarX;
- Sx(id1,:) = Sx(id1,:) * A(:,:,t-1);
- id1 = (t-1)*nbVarX+1:t*nbVarX;
- id2 = 1:(t-1)*nbVarU;
-% N = blkdiag(B(:,:,t-1), N);
- N = blkdiag(N, B(:,:,t-1)); %To be checked!!!
- Su(id1,id2) = M * N;
- M = [A(:,:,t-1) * M, A(:,:,t-1)]; %Also M = [A^(n-1), M] or M = [Sx(id1,:), M]
+ Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+ for t=1:nbData-1
+ id1 = (t-1)*nbVarX+1:t*nbVarX;
+ id2 = t*nbVarX+1:(t+1)*nbVarX;
+ id3 = (t-1)*nbVarU+1:t*nbVarU;
+ Sx(id2,:) = squeeze(A(:,:,t)) * Sx(id1,:);
+ Su(id2,:) = squeeze(A(:,:,t)) * Su(id1,:);
+ Su(id2,id3) = B(:,:,t);
end
end
\ No newline at end of file
diff --git a/demos/demo_MPC_iterativeLQR01.m b/demos/demo_MPC_iterativeLQR01.m
index 1c38126..f762004 100644
--- a/demos/demo_MPC_iterativeLQR01.m
+++ b/demos/demo_MPC_iterativeLQR01.m
@@ -138,12 +138,12 @@ for t=nbData-1:-1:1
end
%Reproduction with feedback (FB) and feedforward (FF) terms
-X = zeros(nbVar,1);
+x = zeros(nbVar,1);
rx(:,1) = X; %Log data
for t=2:nbData
- u = -K(:,:,t-1) * X + Kff(:,:,t-1) * v(:,t); %Acceleration command with FB and FF terms
- X = A * X + B * u; %Update of state vector
- rx(:,t) = X; %Log data
+ u = -K(:,:,t-1) * x + Kff(:,:,t-1) * v(:,t); %Acceleration command with FB and FF terms
+ x = A * x + B * u; %Update of state vector
+ rx(:,t) = x; %Log data
end
diff --git a/demos/demo_MPC_iterativeLQR_augmSigma01.m b/demos/demo_MPC_iterativeLQR_augmSigma01.m
index 19af4c1..f5b305e 100644
--- a/demos/demo_MPC_iterativeLQR_augmSigma01.m
+++ b/demos/demo_MPC_iterativeLQR_augmSigma01.m
@@ -183,7 +183,7 @@ end
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot position
-figure('position',[10 10 1000 1000],'color',[1 1 1]); hold on;
+figure('position',[10 10 1000 1000],'color',[1 1 1]); axis off; hold on;
plotGMM(model0.Mu(1:2,:), model0.Sigma(1:2,1:2,:), [0.5 0.5 0.5], .3);
for n=1:nbSamples
plot(Data(1,(n-1)*nbData+1:n*nbData), Data(2,(n-1)*nbData+1:n*nbData), '-','color',[.7 .7 .7]);
@@ -193,7 +193,6 @@ for n=1:nbRepros
h(2) = plot(r2(n).Data(1,:), r2(n).Data(2,:), '--','linewidth',2,'color',[0 .8 0]);
end
axis equal;
-xlabel('x_1'); ylabel('x_2');
legend(h,'LQR with augmented state space',' LQT with FB and FF terms');
% %Plot velocity
diff --git a/demos/demo_MPC_robot01.m b/demos/demo_MPC_robot01.m
index 3557df5..adf4402 100644
--- a/demos/demo_MPC_robot01.m
+++ b/demos/demo_MPC_robot01.m
@@ -96,7 +96,7 @@ end
figure('position',[10 10 700 700],'color',[1 1 1],'name','x1-x2 plot'); hold on; axis off;
for t=round(linspace(1,nbData,2))
colTmp = [.9,.9,.9] - [.7,.7,.7] * t/nbData;
- plotArm(r(1).q(:,t), ones(nbDOFs,1)*armLength, [0;0;-10+t*0.1], .02, colTmp);
+ plotArm(r(1).q(:,t), ones(nbDOFs,1)*armLength, [0;0;-10+t*0.1], .06, colTmp);
end
% plotArm(model.Mu, ones(nbDOFs,1)*armLength, [0;0;10], .02, [.8 0 0]);
diff --git a/demos/demo_Riemannian_Gdp_vecTransp01.m b/demos/demo_Riemannian_Gdp_vecTransp01.m
index f0d8029..8338cba 100644
--- a/demos/demo_Riemannian_Gdp_vecTransp01.m
+++ b/demos/demo_Riemannian_Gdp_vecTransp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Gdp_vecTransp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Hd_GMM01.m b/demos/demo_Riemannian_Hd_GMM01.m
index 1c787d9..d59948c 100644
--- a/demos/demo_Riemannian_Hd_GMM01.m
+++ b/demos/demo_Riemannian_Hd_GMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Hd_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Hd_interp01.m b/demos/demo_Riemannian_Hd_interp01.m
index 3f72c2d..903e7d8 100644
--- a/demos/demo_Riemannian_Hd_interp01.m
+++ b/demos/demo_Riemannian_Hd_interp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Hd_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S1_interp01.m b/demos/demo_Riemannian_S1_interp01.m
index fbd0cb8..b017af8 100644
--- a/demos/demo_Riemannian_S1_interp01.m
+++ b/demos/demo_Riemannian_S1_interp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S1_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S1_interp02.m b/demos/demo_Riemannian_S1_interp02.m
index a3e5119..bec2ebb 100644
--- a/demos/demo_Riemannian_S1_interp02.m
+++ b/demos/demo_Riemannian_S1_interp02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S1_interp02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_GMM01.m b/demos/demo_Riemannian_S2_GMM01.m
index f03349e..5182c33 100644
--- a/demos/demo_Riemannian_S2_GMM01.m
+++ b/demos/demo_Riemannian_S2_GMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_GMR01.m b/demos/demo_Riemannian_S2_GMR01.m
index 4d50732..d222454 100644
--- a/demos/demo_Riemannian_S2_GMR01.m
+++ b/demos/demo_Riemannian_S2_GMR01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_GMR02.m b/demos/demo_Riemannian_S2_GMR02.m
index e6ff655..db78bbf 100644
--- a/demos/demo_Riemannian_S2_GMR02.m
+++ b/demos/demo_Riemannian_S2_GMR02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_GMR03.m b/demos/demo_Riemannian_S2_GMR03.m
index f75e191..5f777b4 100644
--- a/demos/demo_Riemannian_S2_GMR03.m
+++ b/demos/demo_Riemannian_S2_GMR03.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR03
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_GMR04.m b/demos/demo_Riemannian_S2_GMR04.m
index 304b07d..b1c9fe5 100644
--- a/demos/demo_Riemannian_S2_GMR04.m
+++ b/demos/demo_Riemannian_S2_GMR04.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GMR04
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_GaussProd01.m b/demos/demo_Riemannian_S2_GaussProd01.m
index 7ac4ee6..315ab5c 100644
--- a/demos/demo_Riemannian_S2_GaussProd01.m
+++ b/demos/demo_Riemannian_S2_GaussProd01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_GaussProd01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_TPGMM01.m b/demos/demo_Riemannian_S2_TPGMM01.m
index 0a50824..81489ec 100644
--- a/demos/demo_Riemannian_S2_TPGMM01.m
+++ b/demos/demo_Riemannian_S2_TPGMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_TPGMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_TPGMM02.m b/demos/demo_Riemannian_S2_TPGMM02.m
index ef41fa2..177a7a0 100644
--- a/demos/demo_Riemannian_S2_TPGMM02.m
+++ b/demos/demo_Riemannian_S2_TPGMM02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_TPGMM02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_batchLQR01.m b/demos/demo_Riemannian_S2_batchLQR01.m
index 9abce44..3316c25 100644
--- a/demos/demo_Riemannian_S2_batchLQR01.m
+++ b/demos/demo_Riemannian_S2_batchLQR01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_S2_batchLQR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_batchLQR02.m b/demos/demo_Riemannian_S2_batchLQR02.m
index 380c787..a703b46 100644
--- a/demos/demo_Riemannian_S2_batchLQR02.m
+++ b/demos/demo_Riemannian_S2_batchLQR02.m
@@ -5,7 +5,7 @@ function demo_Riemannian_S2_batchLQR02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_batchLQR03.m b/demos/demo_Riemannian_S2_batchLQR03.m
index db0578c..9730422 100644
--- a/demos/demo_Riemannian_S2_batchLQR03.m
+++ b/demos/demo_Riemannian_S2_batchLQR03.m
@@ -5,7 +5,7 @@ function demo_Riemannian_S2_batchLQR03
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
index ef82dd7..80503b5 100644
--- a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
+++ b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
@@ -4,7 +4,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.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_infHorLQR01.m b/demos/demo_Riemannian_S2_infHorLQR01.m
index 361f79c..c1d6f02 100644
--- a/demos/demo_Riemannian_S2_infHorLQR01.m
+++ b/demos/demo_Riemannian_S2_infHorLQR01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_infHorLQR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S2_vecTransp01.m b/demos/demo_Riemannian_S2_vecTransp01.m
index 54dc29e..8a0ad72 100644
--- a/demos/demo_Riemannian_S2_vecTransp01.m
+++ b/demos/demo_Riemannian_S2_vecTransp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_vecTransp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S3_GMM01.m b/demos/demo_Riemannian_S3_GMM01.m
index ecf960c..933cac0 100644
--- a/demos/demo_Riemannian_S3_GMM01.m
+++ b/demos/demo_Riemannian_S3_GMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S3_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S3_GMR01.m b/demos/demo_Riemannian_S3_GMR01.m
index c0662cf..a943b31 100644
--- a/demos/demo_Riemannian_S3_GMR01.m
+++ b/demos/demo_Riemannian_S3_GMR01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S3_GMR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S3_GMR02.m b/demos/demo_Riemannian_S3_GMR02.m
index 6566f25..10a53d1 100644
--- a/demos/demo_Riemannian_S3_GMR02.m
+++ b/demos/demo_Riemannian_S3_GMR02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S3_GMR02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S3_infHorLQR01.m b/demos/demo_Riemannian_S3_infHorLQR01.m
index 66e1278..c6d87e8 100644
--- a/demos/demo_Riemannian_S3_infHorLQR01.m
+++ b/demos/demo_Riemannian_S3_infHorLQR01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S3_infHorLQR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S3_interp01.m b/demos/demo_Riemannian_S3_interp01.m
index 979fc7e..6b3a799 100644
--- a/demos/demo_Riemannian_S3_interp01.m
+++ b/demos/demo_Riemannian_S3_interp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S3_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_S3_vecTransp01.m b/demos/demo_Riemannian_S3_vecTransp01.m
index 84e9e97..9f84000 100644
--- a/demos/demo_Riemannian_S3_vecTransp01.m
+++ b/demos/demo_Riemannian_S3_vecTransp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S3_vecTransp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_SE2_GMM01.m b/demos/demo_Riemannian_SE2_GMM01.m
index 48a0e3f..eb975b1 100644
--- a/demos/demo_Riemannian_SE2_GMM01.m
+++ b/demos/demo_Riemannian_SE2_GMM01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_SE2_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_SE2_interp01.m b/demos/demo_Riemannian_SE2_interp01.m
index bb95e0e..83959a4 100644
--- a/demos/demo_Riemannian_SE2_interp01.m
+++ b/demos/demo_Riemannian_SE2_interp01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_SE2_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_SOd_interp01.m b/demos/demo_Riemannian_SOd_interp01.m
index 2f91b98..d88cac4 100644
--- a/demos/demo_Riemannian_SOd_interp01.m
+++ b/demos/demo_Riemannian_SOd_interp01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_SOd_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_GMM01.m b/demos/demo_Riemannian_Sd_GMM01.m
index 85a41a3..ebe4374 100644
--- a/demos/demo_Riemannian_Sd_GMM01.m
+++ b/demos/demo_Riemannian_Sd_GMM01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_GMM02.m b/demos/demo_Riemannian_Sd_GMM02.m
index 7241b0d..ea001c0 100644
--- a/demos/demo_Riemannian_Sd_GMM02.m
+++ b/demos/demo_Riemannian_Sd_GMM02.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_GMM02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_GMR01.m b/demos/demo_Riemannian_Sd_GMR01.m
index 3018bd4..766007b 100644
--- a/demos/demo_Riemannian_Sd_GMR01.m
+++ b/demos/demo_Riemannian_Sd_GMR01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_GMR01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_GMR02.m b/demos/demo_Riemannian_Sd_GMR02.m
index 7b3ab37..2d088d3 100644
--- a/demos/demo_Riemannian_Sd_GMR02.m
+++ b/demos/demo_Riemannian_Sd_GMR02.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_GMR02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_GaussProd01.m b/demos/demo_Riemannian_Sd_GaussProd01.m
index 9333597..7b5dedf 100644
--- a/demos/demo_Riemannian_Sd_GaussProd01.m
+++ b/demos/demo_Riemannian_Sd_GaussProd01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GaussProd01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_MPC01.m b/demos/demo_Riemannian_Sd_MPC01.m
index a004551..b5f38b8 100644
--- a/demos/demo_Riemannian_Sd_MPC01.m
+++ b/demos/demo_Riemannian_Sd_MPC01.m
@@ -7,7 +7,7 @@ function demo_Riemannian_Sd_MPC01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_MPC_infHor01.m b/demos/demo_Riemannian_Sd_MPC_infHor01.m
index 95fe04e..7a1f39c 100644
--- a/demos/demo_Riemannian_Sd_MPC_infHor01.m
+++ b/demos/demo_Riemannian_Sd_MPC_infHor01.m
@@ -5,7 +5,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.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_interp01.m b/demos/demo_Riemannian_Sd_interp01.m
index 76f8432..3048a65 100644
--- a/demos/demo_Riemannian_Sd_interp01.m
+++ b/demos/demo_Riemannian_Sd_interp01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_interp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_interp02.m b/demos/demo_Riemannian_Sd_interp02.m
index 662d1d8..76f58a1 100644
--- a/demos/demo_Riemannian_Sd_interp02.m
+++ b/demos/demo_Riemannian_Sd_interp02.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_interp02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_vecTransp01.m b/demos/demo_Riemannian_Sd_vecTransp01.m
index eddd03c..24b7529 100644
--- a/demos/demo_Riemannian_Sd_vecTransp01.m
+++ b/demos/demo_Riemannian_Sd_vecTransp01.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_vecTransp01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_Sd_vecTransp02.m b/demos/demo_Riemannian_Sd_vecTransp02.m
index 751f6cf..f51de09 100644
--- a/demos/demo_Riemannian_Sd_vecTransp02.m
+++ b/demos/demo_Riemannian_Sd_vecTransp02.m
@@ -5,7 +5,7 @@ function demo_Riemannian_Sd_vecTransp02
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_Riemannian_pose_GMM01.m b/demos/demo_Riemannian_pose_GMM01.m
index 64866cf..6efbcf1 100644
--- a/demos/demo_Riemannian_pose_GMM01.m
+++ b/demos/demo_Riemannian_pose_GMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_pose_GMM01
% If this code is useful for your research, please cite the related publication:
% @article{Calinon20RAM,
% author="Calinon, S.",
-% title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
+% title="Gaussians on {R}iemannian Manifolds: Applications for Robot Learning and Adaptive Control",
% journal="{IEEE} Robotics and Automation Magazine ({RAM})",
% year="2020",
% pages=""
diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m
index 3a5902f..e3b3678 100644
--- a/demos/demo_ergodicControl_1D01.m
+++ b/demos/demo_ergodicControl_1D01.m
@@ -36,17 +36,17 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 8000; %Number of datapoints
-nbFct = 50; %Number of basis functions along x and y
+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
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 = 1E0; %Maximum speed allowed
%Desired spatial distribution represented as a mixture of Gaussians
-Mu(:,1) = 0.7 *1;
+Mu(:,1) = 0.7;
Sigma(:,1) = 0.003;
Mu(:,2) = 0.5;
Sigma(:,2) = 0.01;
@@ -56,15 +56,15 @@ Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
%% Compute Fourier series coefficients w_hat of desired spatial distribution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
rg = [0:nbFct-1]';
+kk = rg .* om;
Lambda = (rg.^2 + 1).^-1; %Weighting vector (Eq.(15)
%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)
+ w_hat = w_hat + Priors(j) .* cos(kk .* Mu(:,j)) .* exp(-.5 * kk.^2 .* Sigma(:,j)); %Eq.(22)
end
-w_hat = w_hat ./ L;
+w_hat = w_hat / L;
% %Alternative computation of w_hat by discretization (for verification)
% nbRes = 1000;
@@ -73,20 +73,20 @@ w_hat = w_hat ./ L;
% 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;
+% phi_inv = cos(kk * x) ./ 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));
+% % w_hat(n) = sum(g .* cos(x .* kk(n)));
% % end
% % w_hat = w_hat ./ L ./ nbRes; %Fourier coefficients of spatial distribution
%Fourier basis functions (for a discretized map)
-nbRes = 100;
+nbRes = 200;
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;
+phim = cos(kk * xm) * 2; %Fourier basis functions
+phim(2:end,:) = phim(2:end,:) * 2;
%Desired spatial distribution
g = w_hat' * phim;
@@ -107,31 +107,47 @@ 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; %Gradient of basis functions
+ phi = cos(x * kk); %Eq.(18)
+ dphi = -sin(x * kk) .* kk; %Gradient of basis functions
- wt = wt + phi ./ L; %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 = -dphi' * (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
+ 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
+ x = x + u * dt; %Update of position
+ r.g(:,t) = (wt/t)' * phim; %Reconstructed spatial distribution (for visualization)
+ r.w(:,t) = wt/t; %Fourier coefficients along trajectory (for visualization)
+% r.e(t) = sum(sum((wt/t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
end
+
+% vt = (vt+dt*phi)
+% Bk = sum( hadamard_product(Lambda, (vt/t-w_hat)*t, dphi_k) )
+% B = [Bk k=1,..,d]
+% uk = -umax*Bk/norm(B), k=1,,,d
+
+
+
+% %The Fourier series coefficients along trajectory can alternatively be computed in batch form
+% % wt2 = sum(cos(r.x' * kk'))' ./ L;
+% wt2 = cos(kk * r.x) * ones(nbData,1) ./ L;
+% norm(wt - wt2)
+% return
+
+
% %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);
+% g = g + w_hat(n) .* cos(xm .* kk(n));
% end
% r.g = zeros(1,nbRes);
% for n=1:nbFct
-% r.g = r.g + (wt(n)./nbData) .* cos(xm .* rg(n) .* om);
+% r.g = r.g + (wt(n)./nbData) .* cos(xm .* kk(n));
% end
@@ -139,79 +155,137 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s = nbData; %Reconstruction at time step s
T = nbData; %Size of time window
-figure('position',[10,10,1200,1200],'color',[1 1 1]);
+figure('position',[10,10,2200,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(:,s), '-','linewidth',3,'color',[0 0 0]);
+subplot(3,2,1); hold on;
+h(1) = plot(xm, g, '-','linewidth',4,'color',[.8 0 0]);
+h(2) = plot(xm, r.g(:,s), '-','linewidth',2,'color',[0 0 0]);
+% plot(xm, (r.g(:,s)-r.g(:,s-1)).*1E2, '-','linewidth',3,'color',[0 0 .8]); %Plot increment (scaled)
legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest');
axis([xlim', -.3, max(g)*1.2]);
+set(gca,'xtick',[],'ytick',[],'linewidth',2);
xlabel('$x$','interpreter','latex','fontsize',28);
ylabel('$g(x)$','interpreter','latex','fontsize',28);
-set(gca,'linewidth',2,'xtick',[],'ytick',[]);
+
+%Plot Fourier coefficients
+subplot(3,2,2); hold on;
+plot(rg, zeros(nbFct,1), '-','linewidth',1,'color',[0 0 0]);
+plot(rg, w_hat, '.','markersize',26,'color',[.8 0 0]);
+for n=1:nbFct
+ plot([rg(n), rg(n)], [0, w_hat(n)], '-','linewidth',4,'color',[.8 0 0]);
+end
+plot(rg, r.w(:,s), '.','markersize',18,'color',[0 0 0]);
+for n=1:nbFct
+ plot([rg(n), rg(n)], [0, r.w(n,s)], '-','linewidth',2,'color',[0 0 0]);
+end
+% plot(rg, (r.w(:,s)-r.w(:,s-1)).*1E2, 'o','linewidth',2,'markersize',7,'color',[0 0 .8]); %Plot increment (scaled)
+axis([0, nbFct-1, min(w_hat)-5E-2, max(w_hat)+5E-2]);
+set(gca,'xtick',[0,nbFct-1],'ytick',[],'linewidth',2,'fontsize',20);
+xlabel('$k$','interpreter','latex','fontsize',28);
+ylabel('$w_k$','interpreter','latex','fontsize',28);
+
+%Plot Lambda_k
+subplot(3,2,4); hold on;
+plot(rg, Lambda, '-','linewidth',2,'color',[0 0 0]);
+plot(rg, Lambda, '.','markersize',18,'color',[0 0 0]);
+axis([0, nbFct-1, min(Lambda)-5E-2, max(Lambda)+5E-2]);
+set(gca,'xtick',[0,nbFct-1],'ytick',[0,1],'linewidth',2,'fontsize',20);
+xlabel('$k$','interpreter','latex','fontsize',28);
+ylabel('$\Lambda_k$','interpreter','latex','fontsize',28);
+
%Plot signal
-subplot(3,1,[2,3]); hold on; box on;
+subplot(3,2,[3,5]); 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);
+xlabel('$x$','interpreter','latex','fontsize',28);
% 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');
+% %% Additional plots (static)
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% % %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]);
+% s0 = 2; %Animation from time step s0
+% figure('position',[10,10,2200,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]);
+% subplot(3,2,1); hold on;
% 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 Fourier coefficients
+% subplot(3,2,2); hold on;
+% plot(rg, zeros(nbFct,1), '-','linewidth',1,'color',[0 0 0]);
+% plot(rg, w_hat, '.','markersize',26,'color',[.8 0 0]);
+% for n=1:nbFct
+% plot([rg(n), rg(n)], [0, w_hat(n)], '-','linewidth',4,'color',[.8 0 0]);
+% end
+% axis([0, nbFct-1, min(w_hat)-5E-2, max(w_hat)+5E-2]);
+% set(gca,'xtick',[0,nbFct-1],'ytick',[],'linewidth',2,'fontsize',20);
+% xlabel('$k$','interpreter','latex','fontsize',28);
+% ylabel('$w_k$','interpreter','latex','fontsize',28);
+%
% %Plot signal
-% subplot(3,1,[2,3]); hold on; box on;
+% subplot(3,2,[3,5]); 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
+% hs = [];
+% ha = [];
+% id = 1;
+% for s = s0:5: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;
+% subplot(3,2,1); hold on;
+% delete(ha);
+% ha(1) = plot(xm, g, '-','linewidth',4,'color',[.8 0 0]);
+% ha(2) = plot(xm, r.g(:,s), '-','linewidth',2,'color',[0 0 0]);
+% % ha(3) = plot(xm, (r.g(:,s)-r.g(:,s-1)) .* s .* 5E-2, '-','linewidth',3,'color',[0 0 .8]); %Plot increment (scaled)
+% legend(ha,{'Desired','Reconstructed','Increment (scaled)'},'fontsize',18,'location','northwest');
+%
+% %Plot Fourier coefficients
+% subplot(3,2,2); hold on;
% delete(hs);
+% hs = plot(rg, r.w(:,s), '.','markersize',18,'color',[0 0 0]);
+% for n=1:nbFct
+% hs = [hs, plot([rg(n), rg(n)], [0, r.w(n,s)], '-','linewidth',2,'color',[0 0 0])];
+% end
+% % hs = [hs, plot(rg, (r.w(:,s)-r.w(:,s-1)).*1E2, 'o','linewidth',2,'markersize',7,'color',[0 0 .8])]; %Plot increment (scaled)
+%
+% %Plot signal
+% subplot(3,2,[3,5]); hold on; box on;
% if s>T
-% hs(1) = plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]);
+% hs = [hs, 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]);
+% hs = [hs, 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);
+% hs = [hs, plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0])];
+% drawnow;
+% % print('-dpng',['graphs/anim/ergodicControl_1Danim' num2str(id,'%.4d') '.png']);
+% % pause(1);
+% % id = id + 1;
% end
pause;
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index 0a6858c..9b7ecaf 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -36,8 +36,8 @@ addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 8000; %Number of datapoints
-nbFct = 20; %Number of basis functions along x and y
+nbData = 4000; %Number of datapoints
+nbFct = 6; %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
@@ -45,13 +45,17 @@ 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 = 5E1; %Maximum speed allowed
+u_max = 1E1; %Maximum speed allowed
%Desired spatial distribution represented as a mixture of Gaussians
-Mu(:,1) = [.3; .7];
-Sigma(:,:,1) = eye(nbVar).*1E-2;
-Mu(:,2) = [.7; .4];
-Sigma(:,:,2) = [.1;.2]*[.1;.2]' .*8E-1 + eye(nbVar)*1E-3;
+% Mu(:,1) = [.3; .7];
+% 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;
Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
@@ -91,7 +95,7 @@ w_hat = w_hat ./ L^nbVar ./ size(op,2);
% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
%Fourier basis functions (for a discretized map)
-nbRes = 40;
+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
@@ -135,68 +139,113 @@ for t=1:nbData
u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
x = x + u .* dt; %Update of position
- r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution
+ r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution (for visualization)
+ r.w(:,t) = wt./t; %Fourier coefficients along trajectory (for visualization)
% 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))
+% phi1(1,:,:) = cos(r.x(1:2:end)' * rg .* om);
+% phi1(2,:,:) = cos(r.x(2:2:end)' * rg .* om);
+% wt2 = sum( (phi1(1,:,xx) .* phi1(2,:,yy)) ) ./ L.^nbVar;
+% norm(wt-wt2(:))
+% return
+
%% Plot (static)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1200,1200]); hold on; axis off;
+figure('position',[10,10,2200,800]);
+colormap(repmat(linspace(1,.4,64),3,1)');
+
+%x
+subplot(1,3,1); hold on; axis off;
colormap(repmat(linspace(1,.4,64),3,1)');
-% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
-surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+G = reshape(g,[nbRes,nbRes]);
+% G = reshape(r.g(:,end),[nbRes,nbRes]);
+G([1,end],:) = max(g);
+G(:,[1,end]) = max(g);
+surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), G, 'FaceColor','interp','EdgeColor','interp');
+% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
% plotGMM(Mu, Sigma, [.2 .2 .2], .3);
plot(r.x(1,:), r.x(2,:), '-','linewidth',1,'color',[0 0 0]);
+% plot(r.x(1,:), r.x(2,:), '.','markersize',10,'color',[0 0 0]);
plot(r.x(1,1), r.x(2,1), '.','markersize',15,'color',[0 0 0]);
axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal;
+
+%w
+subplot(1,3,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20);
+imagesc(reshape(wt./t,[nbFct,nbFct]));
+axis tight; axis equal; axis ij;
+
+%w_hat
+subplot(1,3,3); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20);
+imagesc(reshape(w_hat,nbFct,nbFct));
+axis tight; axis equal; axis ij;
% print('-dpng','graphs/ergodicControl_2D01.png');
-% %Plot g as image
-% figure; hold on;
-% imagesc(reshape(g,[nbRes,nbRes])');
-% axis tight; axis equal; %axis ij;
-
-% %Plot g as graph
-% figure('position',[10,10,2300,1300]);
-% subplot(1,2,1); hold on; rotate3d on;
-% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
-% view(3); axis vis3d;
-% subplot(1,2,2); hold on; rotate3d on;
-% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
-% view(3); axis vis3d;
-
-% %Plot Fourier series coefficients
-% figure('position',[1220,10,1300,700]);
-% colormap(repmat(linspace(1,.4,64),3,1)');
-% 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('$w$','interpreter','latex','fontsize',20);
-% imagesc(reshape(wt./t,[nbFct,nbFct]));
-% axis tight; axis equal; axis ij;
+
+% %% Additional plots (static)
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% % %Plot g as image
+% % figure; hold on;
+% % imagesc(reshape(g,[nbRes,nbRes])');
+% % 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([w_hat, reshape(wt./t,[nbFct,nbFct])]);
-% % axis tight; axis equal; axis ij;
+% % %Plot g as graph
+% % figure('position',[10,10,2300,1300]);
+% % subplot(1,2,1); hold on; rotate3d on;
+% % surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+% % view(3); axis vis3d;
+% % subplot(1,2,2); hold on; rotate3d on;
+% % surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+% % view(3); axis vis3d;
% %% Plot (animated)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% figure('position',[10,10,1200,1200]); hold on; axis off;
+% figure('position',[10,10,2200,800]);
+% %x
+% subplot(1,3,1); hold on; axis off;
% colormap(repmat(linspace(1,.4,64),3,1)');
+% G = reshape(g,[nbRes,nbRes]);
+% G([1,end],:) = max(g);
+% G(:,[1,end]) = max(g);
+% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), G, 'FaceColor','interp','EdgeColor','interp');
% plot(r.x(1,1), r.x(2,1), '.','markersize',15,'color',[0 0 0]);
% axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal;
-% h=[]; id=1;
-% for t=1:10:nbData
+% %w
+% subplot(1,3,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20);
+% colormap(repmat(linspace(1,.4,64),3,1)');
+% imagesc(reshape(w_hat,nbFct,nbFct));
+% % surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape((r.g(:,end)-r.g(:,end-1)).*1E2,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp'); %Plot increment
+% axis tight; axis equal; axis ij;
+% %w_hat
+% subplot(1,3,3); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20);
+% colormap(repmat(linspace(1,.4,64),3,1)');
+% imagesc(reshape(w_hat,nbFct,nbFct));
+% axis tight; axis equal; axis ij;
+% %Animation
+% h = [];
+% % id = 1;
+% for t=2:50:nbData
% delete(h);
-% h(1) = surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,t),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
-% h(2) = plot(r.x(1,1:t), r.x(2,1:t), '-','linewidth',1,'color',[0 0 0]);
+% h=[];
+% %x
+% subplot(1,3,1); hold on; axis off;
+% % h = [h, surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,t),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp')];
+% h = [h, plot(r.x(1,1:t), r.x(2,1:t), '-','linewidth',1,'color',[0 0 0])];
+% %w
+% subplot(1,3,2); hold on; axis off;
+% h = [h, imagesc(reshape(r.w(:,t),nbFct,nbFct))];
+% % h = [h, imagesc(reshape((r.w(:,t)-r.w(:,t-1)).*1E2, nbFct, nbFct))]; %Plot increment (scaled)
+% % h = h, surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape((r.g(:,t)-r.g(:,t-1)).*1E2,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp')]; %Plot increment
% drawnow;
-% % print('-dpng',['graphs/anim/ergodicControl_2Danim' num2str(id,'%.3d') '.png']);
-% % id = id + 1;
+% % print('-dpng',['graphs/anim/ergodicControl_2Danim' num2str(id,'%.4d') '.png']);
+% % pause(.5);
+% % [t id]
+% % id = id + 1;
% end
pause;
diff --git a/demos/demo_ergodicControl_3D01.m b/demos/demo_ergodicControl_3D01.m
index 199e653..4f30f04 100644
--- a/demos/demo_ergodicControl_3D01.m
+++ b/demos/demo_ergodicControl_3D01.m
@@ -119,7 +119,7 @@ x = [.7; .1; .5]; %Initial position
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)
phi1 = cos(x * rg .* om); %Eq.(18)
dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
@@ -135,7 +135,7 @@ for t=1:nbData
u = u .* u_max ./ (norm(u)+1E-1); %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
diff --git a/demos/demo_proMP01.m b/demos/demo_proMP01.m
index 1d7f8dd..43592e3 100644
--- a/demos/demo_proMP01.m
+++ b/demos/demo_proMP01.m
@@ -128,8 +128,8 @@ for k=1:3
for n=1:nbRepros
m(k).Mu2(in,n) = x(in,1) + repmat((rand(nbVar,1)-0.5)*2, length(in0), 1) ;
- % %Conditional distribution with trajectory distribution
- % m(k).Mu2(out,n) = m(k).Mu(out) + m(k).Sigma(out,in) / m(k).Sigma(in,in) * (m(k).Mu2(in,n) - m(k).Mu(in));
+% %Conditional distribution with trajectory distribution
+% m(k).Mu2(out,n) = m(k).Mu(out) + m(k).Sigma(out,in) / m(k).Sigma(in,in) * (m(k).Mu2(in,n) - m(k).Mu(in));
%Efficient computation of conditional distribution by exploiting ProMP structure
m(k).Mu2(out,n) = m(k).Psi(out,:) * ...
--
GitLab