diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 0a656bd33ea11416c4d988c82d0c37e137eaaddd..248debe024deeb9d7dac17e682a261b67a53a989 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 2ce347cbebd4d877ff310a5f13e837ef05e9dd0d..365f8f3f83430e7a09819494af43ca85b81979cd 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -2789,14 +2789,16 @@ By defining $\bm{\tilde{R}}=\begin{bmatrix}\bm{0}&\bm{0}\\\bm{0}&\bm{R}\end{bmat
\end{align}
where $\bm{f}(\bm{x})$ denotes the concatenation of $\bm{f}(\bm{x}_t)$ for all time steps.
-If only some parts of $\bm{x}_1$ need to be estimated, the corresponding diagonal entries in $\bm{\tilde{R}}$ can be set with arbitrary high values, by reformulating the minimization problem as
-\begin{align}
- \min_{\bm{\tilde{u}}} \bm{f}(\bm{x})^{\!\trsp} \bm{Q} \bm{f}(\bm{x})
- + {(\bm{\tilde{u}}-\bm{\mu})}^\trsp \! \bm{\tilde{R}} (\bm{\tilde{u}}-\bm{\mu}),
-\end{align}
-where $\bm{\mu}$ is a vector containing the initial elements in $\bm{x}_1$ that do not need to be estimated, where the other elements are zero.
+The approach can for example be used to estimate the optimal placement of a robot manipulator. For a planar robot, this can for example be implemented by defining the kinematic chain starting with two prismatic joints along the two axes directions, providing a way to represent the location of a free floating platform as two additional prismatic joints in the kinematic chain. If we would like the base of the robot to remain static, we can request that these first two prismatic joints will stay still during the motion by removing them from the list of controlled variables updated by iLQR, see Figure \ref{fig:iLQR_initStateOptim} and the corresponding source code example.
+
+%If only some parts of $\bm{x}_1$ need to be estimated, the corresponding diagonal entries in $\bm{\tilde{R}}$ can be set with arbitrary high values, by reformulating the minimization problem as
+%\begin{align}
+% \min_{\bm{\tilde{u}}} \bm{f}(\bm{x})^{\!\trsp} \bm{Q} \bm{f}(\bm{x})
+% + {(\bm{\tilde{u}}-\bm{\mu})}^\trsp \! \bm{\tilde{R}} (\bm{\tilde{u}}-\bm{\mu}),
+%\end{align}
+%where $\bm{\mu}$ is a vector containing the initial elements in $\bm{x}_1$ that do not need to be estimated, where the other elements are zero.
-The above approach can for example be used to estimate the optimal placement of a robot manipulator. For a planar robot, this can for example be implemented by defining the kinematic chain starting with two prismatic joints along the two axes directions, providing a way to represent the location of a free floating platform as two additional prismatic joints in the kinematic chain. If we would like the base of the robot to remain static, we can request that these first two prismatic joints will stay still during the motion. By using the above formulation, this can be done by setting arbitrary large control weights for these corresponding joints, see Figure \ref{fig:iLQR_initStateOptim} and the corresponding source code example.
+%By using the above formulation, this can be done by setting arbitrary large control weights for these corresponding joints
\newpage
diff --git a/matlab/LQT_recursive.m b/matlab/LQT_recursive.m
index 0eaf800a7b0322f61190bc2a7de9458b66dbf516..e10ca113258e38242da06445a70a916f415385f1 100644
--- a/matlab/LQT_recursive.m
+++ b/matlab/LQT_recursive.m
@@ -75,12 +75,12 @@ end
for n=1:2
x = [zeros(param.nbVar,1); 1]; %Augmented state space
for t=1:param.nbData
- Ka = (B' * P(:,:,t) * B + Ra) \ B' * P(:,:,t) * A; %Feedback gain
- u = -Ka * x; %Feedback control on augmented state (resulting in feedback and feedforward terms on state)
- x = A * x + B * u; %Update of state vector
if t==25 && n==2
x = x + xn; %Simulated noise on the state
end
+ Ka = (B' * P(:,:,t) * B + Ra) \ B' * P(:,:,t) * A; %Feedback gain
+ u = -Ka * x; %Feedback control on augmented state (resulting in feedback and feedforward terms on state)
+ x = A * x + B * u; %Update of state vector
r(n).x(:,t) = x; %Log data
end
end
@@ -92,7 +92,9 @@ h = figure('position',[10 10 800 800],'color',[1 1 1]); axis off; hold on;
plot(r(1).x(1,1), r(1).x(2,1), 'k.','markersize',30);
plot(r(1).x(1,:), r(1).x(2,:), 'k:','linewidth',2);
plot(r(2).x(1,:), r(2).x(2,:), 'k-','linewidth',2);
-plot(r(2).x(1,24:25), r(2).x(2,24:25), 'g.','markersize',20);
+plot(r(2).x(1,:), r(2).x(2,:), 'k.','markersize',12);
+plot(r(2).x(1,24:25), r(2).x(2,24:25), 'g-','linewidth',2); %Perturbation
+plot(r(2).x(1,24:25), r(2).x(2,24:25), 'g.','markersize',20); %Perturbation
plot(Mu(1,:), Mu(2,:), 'r.','markersize',40);
axis equal;
diff --git a/matlab/LQT_recursive_LS.m b/matlab/LQT_recursive_LS.m
index 10aa4230c2aec5d8e23e6d59bc6939a24ef584b9..9cd3db86d44dcba203418d031b1c24daefb6e137 100644
--- a/matlab/LQT_recursive_LS.m
+++ b/matlab/LQT_recursive_LS.m
@@ -94,11 +94,14 @@ end
for n=1:2
x = [zeros(param.nbVar,1); 1]; %Augmented state space
for t=1:param.nbData-1
- u = -Ka(:,:,t) * x; %Feedback control on augmented state (resulting in feedback and feedforward teRas on state)
- x = A * x + B * u; %Update of state vector
if t==25 && n==2
x = x + xn; %Simulated noise on the state
end
+% u = -Ka(:,:,t) * x; %Feedback control on augmented state (resulting in feedforward part and feedback on state)
+ u_ff = -Ka(:,end,t); %Feedforward part on state
+ u = u_ff - Ka(:,1:end-1,t) * x(1:end-1); %Feedforward and feedback part on state
+
+ x = A * x + B * u; %Update of state vector
r(n).x(:,t) = x; %Log data
end
end
@@ -110,7 +113,9 @@ h = figure('position',[10 10 800 800],'color',[1 1 1]); axis off; hold on;
plot(r(1).x(1,1), r(1).x(2,1), 'k.','markersize',30);
plot(r(1).x(1,:), r(1).x(2,:), 'k:','linewidth',2);
plot(r(2).x(1,:), r(2).x(2,:), 'k-','linewidth',2);
-plot(r(2).x(1,24:25), r(2).x(2,24:25), 'g.','markersize',20);
+plot(r(2).x(1,:), r(2).x(2,:), 'k.','markersize',12);
+plot(r(2).x(1,24:25), r(2).x(2,24:25), 'g-','linewidth',2); %Perturbation
+plot(r(2).x(1,24:25), r(2).x(2,24:25), 'g.','markersize',20); %Perturbation
plot(Mu(1,:), Mu(2,:), 'r.','markersize',40);
axis equal;
diff --git a/matlab/ergodic_control_SMC_2D.m b/matlab/ergodic_control_SMC_2D.m
index 3800c7385aa7db6e52041efcac08d7517924d5a2..8c742d847a39d48a38e0e2d2d0b53b8ef6a52797 100644
--- a/matlab/ergodic_control_SMC_2D.m
+++ b/matlab/ergodic_control_SMC_2D.m
@@ -8,6 +8,8 @@ function ergodic_control_SMC_2D
%% This file is part of RCFS <https://rcfs.ch>
%% License: GPL-3.0-only
+%pkg('load','statistics'); %if mvnpdf() is used
+
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -55,17 +57,18 @@ for j=1:nbGaussian
end
w_hat = w_hat / L^nbVar / size(op,2);
-% %Alternative computation of w_hat by discretization
-% nbRes = 100;
-% xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
-% [xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
-% g = zeros(1,nbRes^nbVar);
-% for k=1:nbGaussian
-% %Spatial distribution (you can call pkg('load','statistics') in octave if the package is missing)
-% g = g + Priors(k) * mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))';
-% 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
+%%Alternative computation of w_hat by discretization
+%nbRes = 40;
+%xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
+%[xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
+%g = zeros(1,nbRes^nbVar);
+%for k=1:nbGaussian
+%%Spatial distribution (you can call pkg('load','statistics') in octave if the package is missing)
+%g = g + Priors(k) * mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))';
+%end
+%phi_inv = cos(KX(1,:)' * xm(1,:) * om) .* cos(KX(2,:)' * xm(2,:) * om) / L^nbVar / nbRes^nbVar;
+%%w_hat2 = phi_inv / g; %Fourier coefficients of spatial distribution
+%w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
%Fourier basis functions (for a discretized map)
xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
@@ -74,7 +77,7 @@ phim = cos(KX(1,:)' * xm(1,:) * om) .* cos(KX(2,:)' * xm(2,:) * om) * 2^nbVar; %
[xx, yy] = ndgrid(1:nbFct, 1:nbFct);
hk = [1; 2*ones(nbFct-1,1)];
-HK = hk(xx(:)) .* hk(yy(:));
+HK = hk(xx(:)) .* hk(yy(:));
phim = phim .* repmat(HK,[1,nbRes^nbVar]);
%Desired spatial distribution
@@ -110,7 +113,7 @@ end
% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-h = figure('position',[10,10,1800,600]);
+h = figure('position',[10,10,1600,600]);
%x
ax(1) = subplot(1,3,1); hold on; axis off; title('Path generated by ergodic control','fontsize',20);
diff --git a/matlab/iLQR_manipulator_recursive.m b/matlab/iLQR_manipulator_recursive.m
index 544ad1ba75102676f201bda00ee096aacfdfa257..629c52e40fb6918adb09a89e17adc27cab814770 100644
--- a/matlab/iLQR_manipulator_recursive.m
+++ b/matlab/iLQR_manipulator_recursive.m
@@ -130,7 +130,7 @@ for t=1:param.nbPoints
msh(:,:,t) = param.A(:,:,t) * msh0 + repmat(param.Mu(1:2,t), 1, size(msh0,2));
end
-h = figure('position',[10,10,800,800],'color',[1,1,1]); hold on; axis off;
+h = figure('position',[10,10,800,800],'color',[1,1,1]); hold on; %axis off;
colMat = lines(param.nbPoints);
ftmp = fkin0(x(:,1), param);
@@ -148,9 +148,11 @@ end
ftmp = fkin(x, param);
plot(ftmp(1,:), ftmp(2,:), 'k-','linewidth',2);
+plot(ftmp(1,:), ftmp(2,:), 'k.','markersize',12);
plot(ftmp(1,[1,tl]), ftmp(2,[1,tl]), 'k.','markersize',20);
plot(ftmp(1,tn-1:tn), ftmp(2,tn-1:tn), 'g.','markersize',20); %Perturbation
plot(ftmp(1,tn-1:tn), ftmp(2,tn-1:tn), 'g-','linewidth',3); %Perturbation
+%plot(xref(1,:), xref(2,:), 'b.','markersize',12);
axis equal;
waitfor(h);
diff --git a/matlab/spline1D.m b/matlab/spline1D.m
index e20e2eee8c1487d2c81bb43c43f9819193567e41..481a9cf28d44c75251bbce72ae1daf5bc851f8f5 100644
--- a/matlab/spline1D.m
+++ b/matlab/spline1D.m
@@ -94,7 +94,7 @@ w_vis = kron(param.C, eye(param.nbOut)) * w;
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-h = figure('position',[10 10 2500 1800]);
+h = figure('position',[10 10 1500 800]);
set(gca,'linewidth',2);
clrmap = lines(param.nbFct * param.nbSeg); %Colors for total number of control points
@@ -124,7 +124,7 @@ for n=1:param.nbSeg
tSeg_n = linspace(0, 1/param.nbSeg, param.nbFct) + (n-1)/param.nbSeg;
tSeg = [tSeg, tSeg_n];
end
-plot(tSeg, w_vis, '.', 'markersize',38,'color',[0 0 0]);
+plot(tSeg, w_vis, '.', 'markersize',18,'color',[0 0 0]);
%Plot segments between control points
for n=1:param.nbSeg
id = (n-1)*param.nbFct + [1,2];