diff --git a/README.md b/README.md
index f36a4882ee9a1c5ea2956db0f264678e7b73c84c..f46249625f06b3e0ee6c7e11c67b3c804e693710 100644
--- a/README.md
+++ b/README.md
@@ -38,6 +38,7 @@ The RCFS website also includes interactive exercises: [https://rcfs.ch](https://
 | iLQR_manipulator3D | iLQR (batch formulation) in a 3D workspace applied to a Franka Emika manipulator for a viapoints task | ✅ | ✅ |  |  |
 | iLQR_manipulator_initStateOptim | iLQR applied to a planar manipulator, where both an optimal controller and an optimal robot base location are estimated | ✅ | ✅ |  |  |
 | iLQR_manipulator_recursive | iLQR applied to a planar manipulator for a viapoints task (recursive formulation to find a controller) | ✅ | ✅ |  |  |
+| iLQR_manipulator_recursive_LS | iLQR applied to a planar manipulator for a viapoints task (recursive formulation with least squares) | ✅ |  |  |  |
 | iLQR_manipulator_CoM | iLQR applied to a planar manipulator for a tracking problem involving the center of mass (CoM) and the end-effector (batch formulation) | ✅ | ✅ |  |  |
 | iLQR_manipulator_obstacle | iLQR applied to a planar manipulator for a viapoints task with obstacles avoidance (batch formulation) | ✅ | ✅ |  |  |
 | iLQR_manipulator_CP | iLQR with control primitives applied to a viapoint task with a manipulator (batch formulation) | ✅ | ✅ |  |  |
@@ -59,7 +60,7 @@ The RCFS website also includes interactive exercises: [https://rcfs.ch](https://
 
 RCFS is maintained by Sylvain Calinon, https://calinon.ch
 
-Contributors: Sylvain Calinon, Philip Abbet, Jérémy Maceiras, Hakan Girgin, Julius Jankowski, Teguh Lembono, Tobias Löw, Amirreza Razmjoo, Boyang Ti, Teng Xue, Yifei Dong, Yiming Li, Cem Bilaloglu, Yan Zhang, Guillaume Clivaz, Maximilien Dufau
+Contributors: Sylvain Calinon, Philip Abbet, Jérémy Maceiras, Hakan Girgin, Julius Jankowski, Teguh Lembono, Tobias Löw, Amirreza Razmjoo, Boyang Ti, Teng Xue, Yifei Dong, Yiming Li, Cem Bilaloglu, Yan Zhang, Guillaume Clivaz, Maximilien Dufau, Giovanni Braglia
 
 Copyright (c) 2024 Idiap Research Institute, https://idiap.ch
 
diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 248debe024deeb9d7dac17e682a261b67a53a989..12418fcd96622bf4a862310768196a1adc0648a9 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 365f8f3f83430e7a09819494af43ca85b81979cd..b0b082e34f526f18af69e28b0336fe2913b57593 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -1995,7 +1995,7 @@ An infinite time horizon corresponds to $T\rightarrow\infty$, characterized by t
 	+ \bm{Q} - \bm{A}^\trsp \bm{P}_\infty \bm{B} {( \bm{B}^\trsp \bm{P}_\infty \bm{B} + \bm{R} )}^{-1} \bm{B}^\trsp \bm{P}_\infty \bm{A}.
 \end{equation}
 The above equation can also be solved through a discrete algebraic Riccati equation (DARE). 
-By rewriting the cost in \eqref{eq:PT} with a Lagragian formulation
+By rewriting the cost with a Lagragian formulation %in \eqref{eq:PT} 
 \begin{equation}
 	c = \sum_{t=1}^T \bm{x}_t^\trsp \bm{Q} \bm{x}_t + \bm{u}_t^\trsp \bm{R} \bm{u}_t 
 	+\bm{\lambda}_t (\bm{A} \bm{x}_t + \bm{B}\bm{u}_t - \bm{x}_{t+1}),
@@ -2436,6 +2436,9 @@ The complete iLQR procedure is described in Algorithm \ref{alg:iLQRrecursive} (i
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 \subsection{Least squares formulation of recursive iLQR}\label{sec:iLQRLS}
+\begin{flushright}
+\filename{iLQR\_manipulator\_recursive\_LS.*}
+\end{flushright}
 
 First, note that \eqref{eq:duhat} can be rewritten as
 \begin{equation}
diff --git a/matlab/iLQR_manipulator_recursive.m b/matlab/iLQR_manipulator_recursive.m
index 629c52e40fb6918adb09a89e17adc27cab814770..86f45f4aeb85f1da4145e3cf0d0695dc4e6e16de 100644
--- a/matlab/iLQR_manipulator_recursive.m
+++ b/matlab/iLQR_manipulator_recursive.m
@@ -4,7 +4,7 @@
 %% Copyright (c) 2023 Idiap Research Institute <https://www.idiap.ch/>
 %% Written by Sylvain Calinon <https://calinon.ch>
 %% 
-%% This file is part of RCFS <https://robotics-codes-from-scratch.github.io/>
+%% This file is part of RCFS <https://rcfs.ch/>
 %% License: GPL-3.0-only
 
 function iLQR_manipulator_recursive
@@ -13,7 +13,7 @@ function iLQR_manipulator_recursive
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 param.dt = 1E-2; %Time step length
 param.nbData = 50; %Number of datapoints
-param.nbIter = 100; %Number of iterations for iLQR
+param.nbMaxIter = 100; %Maximum number of iterations for iLQR
 param.nbPoints = 2; %Number of viapoints
 param.nbVarX = 3; %State space dimension (x1,x2,x3)
 param.nbVarU = 3; %Control space dimension (dx1,dx2,dx3)
@@ -41,7 +41,7 @@ B = eye(param.nbVarX, param.nbVarU) * param.dt;
 Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); tril(kron(ones(param.nbData-1), eye(param.nbVarX)*param.dt))];
 Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
 
-%Initializations
+%Initialization
 du = zeros(param.nbVarU, param.nbData-1);
 utmp = zeros(param.nbVarU, param.nbData-1);
 xtmp = zeros(param.nbVarX, param.nbData);
@@ -55,7 +55,7 @@ x0 = [3*pi/4; -pi/2; -pi/4]; %Initial robot pose
 uref = zeros(param.nbVarU, param.nbData-1); %Initial commands
 xref = reshape(Su0 * uref(:) + Sx0 * x0, param.nbVarX, param.nbData); %Initial states
 
-for n=1:param.nbIter
+for n=1:param.nbMaxIter
 	[f, J] = f_reach(xref(:,tl), param); %Residuals and Jacobians
 
 	Lu = uref * param.r;
@@ -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);
@@ -198,23 +198,14 @@ function [f, J] = f_reach(x, param)
 % 	f = fkin(x, param) - param.Mu; %Residuals by ignoring manifold
 	f = logmap(fkin(x, param), param.Mu); %Residuals by considering manifold
 	
-	J = zeros(param.nbVarF, param.nbVarX, param.nbPoints); 
+	J = []; 
 	for t=1:size(x,2)
 		f(1:2,t) = param.A(:,:,t)' * f(1:2,t); %Object-centered forward kinematics
 		
 		Jtmp = Jkin(x(:,t), param);
 		Jtmp(1:2,:) = param.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
 		
-%		%Bounding boxes (optional)
-%		for i=1:2
-%			if abs(f(i,t)) < param.sz(i)
-%				f(i,t) = 0;
-%				Jtmp(i,:) = 0;
-%			else
-%				f(i,t) = f(i,t) - sign(f(i,t)) * param.sz(i);
-%			end
-%		end
-		
+		%J = blkdiag(J, Jtmp);
 		J(:,:,t) = Jtmp;
 	end
 end
diff --git a/matlab/iLQR_manipulator_recursive_LS.m b/matlab/iLQR_manipulator_recursive_LS.m
new file mode 100644
index 0000000000000000000000000000000000000000..53e81b6149f3fde3e9968bb80c5c17d4a1fa4afd
--- /dev/null
+++ b/matlab/iLQR_manipulator_recursive_LS.m
@@ -0,0 +1,216 @@
+%% iLQR applied to a planar manipulator for a viapoints task 
+%% (recursive formulation computed with least squares to find a controller)
+%% 
+%% Copyright (c) 2025 Idiap Research Institute <https://www.idiap.ch/>
+%% Written by Sylvain Calinon <https://calinon.ch> and Giovanni Braglia
+%% 
+%% This file is part of RCFS <https://rcfs.ch/>
+%% License: GPL-3.0-only
+
+function iLQR_manipulator_recursive_LS
+
+%% Parameters
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+param.dt = 1E-2; %Time step length
+param.nbData = 50; %Number of datapoints
+param.nbMaxIter = 100; %Maximum number of iterations for iLQR
+param.nbPoints = 2; %Number of viapoints
+param.nbVarX = 3; %State space dimension (x1,x2,x3)
+param.nbVarU = 3; %Control space dimension (dx1,dx2,dx3)
+param.nbVarF = 3; %Objective function dimension (f1,f2,f3, with f3 as orientation)
+param.l = [2; 2; 1]; %Robot links lengths
+param.sz = [.2, .3]; %Size of objects
+param.q = 1E0; %Tracking weighting term
+param.r = 1E-6; %Control weighting term
+
+param.Mu = [[2; 1; -pi/6], [3; 2; -pi/3]]; %Viapoints 
+for t=1:param.nbPoints
+	param.A(:,:,t) = [cos(param.Mu(3,t)), -sin(param.Mu(3,t)); sin(param.Mu(3,t)), cos(param.Mu(3,t))]; %Object orientation matrices
+end
+
+Q = speye(param.nbVarF * param.nbPoints) * param.q; %Precision matrix
+R = speye((param.nbData-1)*param.nbVarU) * param.r; %Control weight matrix (at trajectory level)
+
+%Time occurrence of viapoints
+tl = linspace(1, param.nbData, param.nbPoints+1);
+tl = round(tl(2:end));
+idx = (tl - 1) * param.nbVarX + [1:param.nchansons quebecoise 2024bVarX]';
+
+
+%% Iterative LQR (iLQR)
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%Transfer matrices (for linear system as single integrator)
+A = eye(param.nbVarX);
+B = eye(param.nbVarX, param.nbVarU) * param.dt;
+A_ = blkdiag(A, 1); %Augmented A
+B_ = [B; zeros(1,param.nbVarU)]; %Augmented B
+
+Su0 = [zeros(param.nbVarX, param.nbVarX*(param.nbData-1)); tril(kron(ones(param.nbData-1), eye(param.nbVarX)*param.dt))];
+Sx0 = kron(ones(param.nbData,1), eye(param.nbVarX));
+Su = Su0(idx,:);
+Sx = Sx0(idx,:);
+
+%Initialization
+du = zeros(param.nbVarU, param.nbData-1);
+utmp = zeros(param.nbVarU, param.nbData-1);
+xtmp = zeros(param.nbVarX, param.nbData);
+k = zeros(param.nbVarU, param.nbData-1);
+K = zeros(param.nbVarU, param.nbVarX, param.nbData-1);
+Luu = repmat(eye(param.nbVarU) * param.r, [1,1,param.nbData]);
+Fx = repmat(A, [1,1,param.nbData]);
+Fu = repmat(B, [1,1,param.nbData]);
+
+x0 = [3*pi/4; -pi/2; -pi/4]; %Initial robot pose
+uref = zeros(param.nbVarU, param.nbData-1); %Initial commands
+xref = reshape(Su0 * uref(:) + Sx0 * x0, param.nbVarX, param.nbData); %Initial states
+
+for n=1:param.nbMaxIter
+	[f, J] = f_reach(xref(:,tl), param); %Residuals and Jacobians
+	
+	Quu_inv = inv(Su' * J' * Q * J * Su + R);
+    Qux =  Su' * J' * Q * J * Sx;
+    qu = Su' * J' * Q * f(:) + R * uref(:);
+    F = Quu_inv * [Qux, qu];
+    
+    %Backward pass
+    Ka(:,:,1) = F(1:param.nbVarU,:);
+    k(:,1) = -Ka(:,end,1);
+    K(:,:,1) = -Ka(:,1:end-1,1);
+    P = eye(param.nbVarX+1);
+    for t=2:param.nbData-1
+	    id = (t-1)*param.nbVarU + [1:param.nbVarU];
+	    P = P / (A_ - B_ * Ka(:,:,t-1));
+	    Ka(:,:,t) = F(id,:) * P; %Feedback gain on augmented state
+        k(:,t) = - Ka(:,end,t);
+        K(:,:,t) = -Ka(:,1:end-1,t);
+    end
+
+    %Forward pass, including step size estimate (backtracking line search method)
+    alpha = 1;
+    cost0 = f(:)' * Q * f(:) + norm(uref(:))^2 * param.r; %u' * R * u
+    while 1
+        xtmp(:,1) = x0;
+        for t=1:param.nbData-1       
+            du(:,t) = alpha * k(:,t) + K(:,:,t) * ( xtmp(:,t) - xref(:,t) );
+	        utmp(:,t) = uref(:,t) + du(:,t);
+	        xtmp(:,t+1) = A * xtmp(:,t) + B * utmp(:,t); %System evolution
+        end
+        ftmp = f_reach(xtmp(:,tl), param); %Residuals 
+        cost = ftmp(:)' * Q * ftmp(:) + norm(utmp(:))^2 * param.r; %Cost
+        
+        if cost < cost0 || alpha < 1E-3
+	        break;
+        end
+        alpha = alpha * 0.5;
+    end
+    uref = uref + du * alpha; %Update control ref
+    xref = reshape(Su0 * uref(:) + Sx0 * x0, param.nbVarX, param.nbData); %System evolution
+    
+	
+	if norm(du(:) * alpha) < 1E-3
+		break; %Stop iLQR when solution is reached
+	end
+end
+disp(['iLQR converged in ' num2str(n) ' iterations.']);
+
+
+%% Simulate reproduction with perturbation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+xn = [.5; zeros(param.nbVarX-1, 1)]; %Simulated perturbation on the state
+tn = round(param.nbData/3); %Time occurrence of perturbation
+x(:,1) = x0;
+for t=1:param.nbData-1
+	if t==tn
+		x(:,t) = x(:,t) + xn; %Simulated perturbation on the state
+	end	
+	u(:,t) = uref(:,t) + K(:,:,t) * (x(:,t) - xref(:,t));
+	x(:,t+1) = A * x(:,t) + B * u(:,t); %System evolution
+end
+
+
+%% Plot state space
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+msh0 = diag(param.sz) * [-1 -1 1 1 -1; -1 1 1 -1 -1];
+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;
+colMat = lines(param.nbPoints);
+
+ftmp = fkin0(x(:,1), param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',4,'color',[.8 .8 .8]);
+
+ftmp = fkin0(x(:,tl(1)), param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',4,'color',[.6 .6 .6]);
+
+ftmp = fkin0(x(:,tl(2)), param);
+plot(ftmp(1,:), ftmp(2,:), '-','linewidth',4,'color',[.4 .4 .4]);
+
+for t=1:param.nbPoints
+	patch(msh(1,:,t), msh(2,:,t), min(colMat(t,:)*1.7,1),'linewidth',2,'edgecolor',colMat(t,:));
+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);
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Logarithmic map for R^2 x S^1 manifold
+function e = logmap(f, f0)
+	e = [f(1:2,:) - f0(1:2,:); ...
+	     imag(log(exp(f0(3,:)*1i)' .* exp(f(3,:)*1i).'))'];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics for end-effector (in robot coordinate system)
+function f = fkin(x, param)
+	L = tril(ones(size(x,1)));
+	f = [param.l' * cos(L * x); ...
+	     param.l' * sin(L * x); ...
+	     mod(sum(x,1)+pi, 2*pi) - pi]; %f1,f2,f3, where f3 is the orientation (single Euler angle for planar robot)
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Forward kinematics for all robot articulations (in robot coordinate system)
+function f = fkin0(x, param)
+	L = tril(ones(size(x,1)));
+	f = [L * diag(param.l) * cos(L * x), ...
+	     L * diag(param.l) * sin(L * x)]'; 
+	f = [zeros(2,1), f];
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Jacobian with analytical computation (for single time step)
+function J = Jkin(x, param)
+	L = tril(ones(size(x,1)));
+	J = [-sin(L * x)' * diag(param.l) * L; ...
+	      cos(L * x)' * diag(param.l) * L; ...
+	      ones(1, size(x,1))]; 
+end
+
+%%%%%%%%%%%%%%%%%%%%%%
+%Residuals f and Jacobians J for a viapoints reaching task (in object coordinate system)
+function [f, J] = f_reach(x, param)
+% 	f = fkin(x, param) - param.Mu; %Residuals by ignoring manifold
+	f = logmap(fkin(x, param), param.Mu); %Residuals by considering manifold
+	
+	J = []; 
+	for t=1:size(x,2)
+		f(1:2,t) = param.A(:,:,t)' * f(1:2,t); %Object-centered forward kinematics
+		
+		Jtmp = Jkin(x(:,t), param);
+		Jtmp(1:2,:) = param.A(:,:,t)' * Jtmp(1:2,:); %Object-centered Jacobian
+		
+		J = blkdiag(J, Jtmp);
+	end
+end