diff --git a/README.md b/README.md
index 6ddc05238b68958f58a5cfd7f3ddaa8d96d7bb18..e83577c903e98b9762e9f07ec361ff2f887005d5 100644
--- a/README.md
+++ b/README.md
@@ -132,12 +132,12 @@ All the examples are located in the `demos` folder, and the functions are locate
 | [demo_manipulabilityTransfer01.m](./demos/demo_manipulabilityTransfer01.m) | [[6]](#ref-6) | Use of robot redundancy to track desired manipulability ellipsoid |
 | [demo_manipulabilityTransfer02.m](./demos/demo_manipulabilityTransfer02.m) | [[6]](#ref-6) | Learning and reproduction of manipulability ellipsoid profiles |
 | [demo_manipulabilityTransfer03.m](./demos/demo_manipulabilityTransfer03.m) | [[6]](#ref-6) | Learning and reproduction of manipulability ellipsoid profiles (numerical version) |
-| [demo_OC_DDP_bicopter01.m](./demos/demo_OC_DDP_bicopter01.m) | [[2]](#ref-2) | iLQR applied to a bicopter problem |
-| [demo_OC_DDP_car01.m](./demos/demo_OC_DDP_car01.m) | [[2]](#ref-2) | iLQR applied to a car parking problem |
-| [demo_OC_DDP_cartpole01.m](./demos/demo_OC_DDP_cartpole01.m) | [[2]](#ref-2) | iLQR applied to a cartpole problem |
-| [demo_OC_DDP_humanoid01.m](./demos/demo_OC_DDP_humanoid01.m) | [[2]](#ref-2) | iLQR applied to a planar 5-link humanoid problem with constraints between joints |
-| [demo_OC_DDP_manipulator01.m](./demos/demo_OC_DDP_manipulator01.m) | [[2]](#ref-2) | iLQR applied to a 2D manipulator problem |
-| [demo_OC_DDP_pendulum01.m](./demos/demo_OC_DDP_pendulum01.m) | [[2]](#ref-2) | iLQR applied to an actuated inverted pendulum problem |
+| [demo_OC_DDP_bicopter01.m](./demos/demo_OC_DDP_bicopter01.m) | [[11]](#ref-11) | iLQR applied to a bicopter problem |
+| [demo_OC_DDP_car01.m](./demos/demo_OC_DDP_car01.m) | [[11]](#ref-11) | iLQR applied to a car parking problem |
+| [demo_OC_DDP_cartpole01.m](./demos/demo_OC_DDP_cartpole01.m) | [[11]](#ref-11) | iLQR applied to a cartpole problem |
+| [demo_OC_DDP_humanoid01.m](./demos/demo_OC_DDP_humanoid01.m) | [[11]](#ref-11) | iLQR applied to a planar 5-link humanoid problem with constraints between joints |
+| [demo_OC_DDP_manipulator01.m](./demos/demo_OC_DDP_manipulator01.m) | [[11]](#ref-11) | iLQR applied to a 2D manipulator problem |
+| [demo_OC_DDP_pendulum01.m](./demos/demo_OC_DDP_pendulum01.m) | [[11]](#ref-11) | iLQR applied to an actuated inverted pendulum problem |
 | [demo_OC_LQT01.m](./demos/demo_OC_LQT01.m) | [[2]](#ref-2) | Batch solution of linear quadratic tracking (LQT) optimal control problem (example with viapoints and simple/double/triple integrator system) |
 | [demo_OC_LQT02.m](./demos/demo_OC_LQT02.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but with a GMM encoding of the reference (example by tracking position and velocity reference) |
 | [demo_OC_LQT03.m](./demos/demo_OC_LQT03.m) | [[2]](#ref-2) | Same as demo_OC_LQT01.m but with a GMM encoding of the reference (example by tracking only position reference) |
@@ -342,6 +342,13 @@ If you find PbDlib useful for your research, please cite the related publication
 <br><strong>(Ref. for machine learning teaching material)</strong>
 </p>
 
+<p><a name="ref-11">
+[11] Lembono, T.S. and Calinon, S. (2021). <strong>Probabilistic Iterative LQR for Short Time Horizon MPC</strong>. arXiv:2012.06349.
+</a><br>
+[[pdf]](https://arxiv.org/pdf/2012.06349.pdf)
+<br><strong>(Ref. for iLQR/DDP)</strong>
+</p>
+
 ### Gallery
 
 |                         |                         |
diff --git a/demos/demo_IK_expForm01.m b/demos/demo_IK_expForm01.m
index c9c61d03b56a0ed030f1c38f717b38b8223d7266..b5f53bff6aadd6247250a23b28678753a01622ef 100644
--- a/demos/demo_IK_expForm01.m
+++ b/demos/demo_IK_expForm01.m
@@ -1,5 +1,5 @@
 function demo_IK_expForm01
-% Forward and inverse kinematics for a planar robot with exp(1i*q) formulation and analytical Jacobian computation
+% Forward and inverse kinematics for a planar robot with exp(1i*q) formulation and analytical Jacobian 
 %
 % Sylvain Calinon, 2020
 
diff --git a/demos/demo_OC_DDP_bicopter01.m b/demos/demo_OC_DDP_bicopter01.m
index ec201b75164f01d9c4c86e35814efe636f821616..760767f7b793f23c9e4990a5aada8300d59241e6 100644
--- a/demos/demo_OC_DDP_bicopter01.m
+++ b/demos/demo_OC_DDP_bicopter01.m
@@ -1,11 +1,31 @@
 function demo_OC_DDP_bicopter01
-% iLQR applied to a bicopter problem
+% iLQR applied to a bicopter problem.
 %
-% Sylvain Calinon, 2020
-%
-% Reference:
-% W. Li and E. Todorov, "Iterative linear quadratic regulator design for nonlinear biological movement systems," 
-%	in Intl Conf. on InfoRatics in Control, Automation and Robotics, 2004, pp. 222–229
+% If this code is useful for your research, please cite the related publication:
+% @article{Lembono21,
+% 	author="Lembono, T. S. and Calinon, S.",
+% 	title="Probabilistic Iterative {LQR} for Short Time Horizon {MPC}",
+% 	year="2021",
+% 	journal="arXiv:2012.06349",
+% 	pages=""
+% }
+% 
+% Copyright (c) 2021 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
+% 
+% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
 
 addpath('./m_fcts/');
 
diff --git a/demos/demo_OC_DDP_car01.m b/demos/demo_OC_DDP_car01.m
index ba5dc0b671d3da6285efb5d111c01d6de064f70b..d94bd55600a7babaed09fc19ab006ba49081af84 100644
--- a/demos/demo_OC_DDP_car01.m
+++ b/demos/demo_OC_DDP_car01.m
@@ -1,11 +1,32 @@
 function demo_OC_DDP_car01
-% iLQR applied to a car parking problem
+% iLQR applied to a car parking problem.
 %
-% Sylvain Calinon, 2020
-%
-% Reference:
-% W. Li and E. Todorov, "Iterative linear quadratic regulator design for nonlinear biological movement systems," 
-%	in Intl Conf. on InfoRatics in Control, Automation and Robotics, 2004, pp. 222–229
+% If this code is useful for your research, please cite the related publication:
+% @article{Lembono21,
+% 	author="Lembono, T. S. and Calinon, S.",
+% 	title="Probabilistic Iterative {LQR} for Short Time Horizon {MPC}",
+% 	year="2021",
+% 	journal="arXiv:2012.06349",
+% 	pages=""
+% }
+% 
+% Copyright (c) 2021 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
+% 
+% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
+
 
 addpath('./m_fcts/');
 
@@ -28,7 +49,7 @@ model.Mu = [4; 3; pi/2; 0]; %Target
 model.l = 0.5; %Length of car
 
 R = speye((model.nbData-1)*model.nbVarU) * model.rfactor; %Control weight matrix (at trajectory level)
-Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
+% Rx = speye(model.nbData*model.nbVarX) * model.rfactor;
 
 Q = blkdiag(sparse((model.nbData-1)*model.nbVarX, (model.nbData-1)*model.nbVarX), speye(model.nbVarX)*1E2); %Sparse precision matrix (at trajectory level)
 MuQ = [sparse((model.nbData-1)*model.nbVarX,1); model.Mu]; %Sparse reference (at trajectory level)
@@ -53,7 +74,7 @@ for n=1:model.nbIter
 	
 	%Linearization
 	[A, B] = linSys(x, u, model);
-	[Su, Sx] = transferMatrices(A, B);
+	Su = transferMatrices(A, B);
 	
 % 	du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:) - Su' * Q * Sx * delta_x0); %Control trajectory
 	du_vec = (Su' * Q * Su + R) \ (Su' * Q * (MuQ - x(:)) - R * u(:)); %Control trajectory
diff --git a/demos/demo_OC_DDP_cartpole01.m b/demos/demo_OC_DDP_cartpole01.m
index 0c3be4c620ec3dcca956a635ea5185f0daaefa24..7df9337edcbf9056786ebd2ebde59f079dc99bd8 100644
--- a/demos/demo_OC_DDP_cartpole01.m
+++ b/demos/demo_OC_DDP_cartpole01.m
@@ -1,10 +1,31 @@
 function demo_OC_DDP_cartpole01
-% iLQR applied to a cartpole problem
+% iLQR applied to a cartpole problem.
 %
-% Sylvain Calinon, 2020
-%
-% Reference for cartpole system:
-% http://databookuw.com/databook.pdf, p. 372
+% If this code is useful for your research, please cite the related publication:
+% @article{Lembono21,
+% 	author="Lembono, T. S. and Calinon, S.",
+% 	title="Probabilistic Iterative {LQR} for Short Time Horizon {MPC}",
+% 	year="2021",
+% 	journal="arXiv:2012.06349",
+% 	pages=""
+% }
+% 
+% Copyright (c) 2021 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
+% 
+% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
 
 addpath('./m_fcts/');
 
diff --git a/demos/demo_OC_DDP_humanoid01.m b/demos/demo_OC_DDP_humanoid01.m
index 419ea951abd72eb4ebdf6864709f377207c922d3..9514f4f1b9f43ad0a9fcd6f77b1a2176f203bceb 100644
--- a/demos/demo_OC_DDP_humanoid01.m
+++ b/demos/demo_OC_DDP_humanoid01.m
@@ -1,7 +1,31 @@
 function demo_OC_DDP_humanoid01
-% iLQR applied to a planar 5-link humanoid problem with constraints between joints
+% iLQR applied to a planar 5-link humanoid problem with constraints between joints.
 %
-% Sylvain Calinon, 2020
+% If this code is useful for your research, please cite the related publication:
+% @article{Lembono21,
+% 	author="Lembono, T. S. and Calinon, S.",
+% 	title="Probabilistic Iterative {LQR} for Short Time Horizon {MPC}",
+% 	year="2021",
+% 	journal="arXiv:2012.06349",
+% 	pages=""
+% }
+% 
+% Copyright (c) 2021 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
+% 
+% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
 
 addpath('./m_fcts/');
 
diff --git a/demos/demo_OC_DDP_manipulator01.m b/demos/demo_OC_DDP_manipulator01.m
index 3420b633ad9995f58b63edc7ac1bdb9735b17858..a80fe6d87520b7478db58877c53a53c22ad4c288 100644
--- a/demos/demo_OC_DDP_manipulator01.m
+++ b/demos/demo_OC_DDP_manipulator01.m
@@ -1,8 +1,32 @@
 function demo_OC_DDP_manipulator01
-% iLQR applied to a 2D manipulator problem with fast computation 
+% iLQR applied to a 2D manipulator problem  
 % (viapoints task with position+orientation, including bounding boxes on f(x))
 %
-% Sylvain Calinon, 2021
+% If this code is useful for your research, please cite the related publication:
+% @article{Lembono21,
+% 	author="Lembono, T. S. and Calinon, S.",
+% 	title="Probabilistic Iterative {LQR} for Short Time Horizon {MPC}",
+% 	year="2021",
+% 	journal="arXiv:2012.06349",
+% 	pages=""
+% }
+% 
+% Copyright (c) 2021 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
+% 
+% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
 
 addpath('./m_fcts/');
 
@@ -16,7 +40,7 @@ model.nbPoints = 2; %Number of viapoints
 model.nbVarX = 3; %State space dimension (q1,q2,q3)
 model.nbVarU = 3; %Control space dimension (dq1,dq2,dq3)
 model.nbVarF = 3; %Objective function dimension (x1,x2,o)
-model.l = [2, 2, 1]; %Links length
+model.l = [2, 2, 1]; %Robot links lengths
 model.sz = [.2, .3]; %Size of objects
 model.r = 1E-4; %Control weight term
 
@@ -49,7 +73,7 @@ for n=1:model.nbIter
 	x = reshape(Su0 * u + Sx0 * x0, model.nbVarX, model.nbData); %System evolution
 	f = fkine(x(:,tl), model);
 	J = jacob(x(:,tl), f, model);
-	du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u(:) * model.r); %Gradient
+	du = (Su' * J' * Q * J * Su + R) \ (-Su' * J' * Q * f(:) - u * model.r); %Gradient
 	
 	%Estimate step size with line search method
 	alpha = 1;
@@ -126,8 +150,9 @@ end
 function [Su, Sx] = transferMatrices(A, B)
 	[nbVarX, nbVarU, nbData] = size(B);
 	nbData = nbData+1;
-	Sx = kron(ones(nbData,1), speye(nbVarX)); 
-	Su = sparse(zeros(nbVarX*(nbData-1), nbVarU*(nbData-1)));
+% 	Sx = kron(ones(nbData,1), speye(nbVarX)); 
+	Sx = speye(nbData*nbVarX, nbVarX); 
+	Su = sparse(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;
@@ -148,6 +173,8 @@ end
 %%%%%%%%%%%%%%%%%%%%%%
 %Forward kinematics (in robot coordinate system)
 function f = fkine0(x, model)
+% 	f = [model.l * cos(tril(ones(model.nbVarU)) * x); ...
+% 			 model.l * sin(tril(ones(model.nbVarU)) * x)];
 	f = model.l * exp(1i * tril(ones(model.nbVarU)) * x); 
 	f = [real(f); imag(f); mod(sum(x)+pi, 2*pi) - pi]; %x1,x2,o (orientation as single Euler angle for planar robot)
 end
@@ -178,6 +205,8 @@ end
 %%%%%%%%%%%%%%%%%%%%%%
 %Jacobian with analytical computation (for single time step)
 function J = jacob0(x, model)
+% 	J = [-sin(tril(ones(model.nbVarU)) * x)' * tril(ones(model.nbVarU)) * diag(model.l); ...
+% 				cos(tril(ones(model.nbVarU)) * x)' * tril(ones(model.nbVarU)) * diag(model.l)];			
 	J = 1i * exp(1i * tril(ones(model.nbVarU)) * x).' * tril(ones(model.nbVarU)) * diag(model.l); 
 	J = [real(J); imag(J); ones(1, model.nbVarX)]; %x1,x2,o
 end
diff --git a/demos/demo_OC_DDP_pendulum01.m b/demos/demo_OC_DDP_pendulum01.m
index fc4e5a1c6e8e98e0abda06058aa387cd2d04597d..4e90b0fc9c4f16239d86e3f29bf61d5b77150825 100644
--- a/demos/demo_OC_DDP_pendulum01.m
+++ b/demos/demo_OC_DDP_pendulum01.m
@@ -1,11 +1,31 @@
 function demo_OC_DDP_pendulum01
-% iLQR applied to an inverted pendulum problem
+% iLQR applied to an inverted pendulum problem.
 %
-% Sylvain Calinon, 2020
-%
-% Reference:
-% W. Li and E. Todorov, "Iterative linear quadratic regulator design for nonlinear biological movement systems," 
-%	in Intl Conf. on InfoRatics in Control, Automation and Robotics, 2004, pp. 222–229
+% If this code is useful for your research, please cite the related publication:
+% @article{Lembono21,
+% 	author="Lembono, T. S. and Calinon, S.",
+% 	title="Probabilistic Iterative {LQR} for Short Time Horizon {MPC}",
+% 	year="2021",
+% 	journal="arXiv:2012.06349",
+% 	pages=""
+% }
+% 
+% Copyright (c) 2021 Idiap Research Institute, https://idiap.ch/
+% Written by Sylvain Calinon, https://calinon.ch/
+% 
+% This file is part of PbDlib, https://www.idiap.ch/software/pbdlib/
+% 
+% PbDlib is free software: you can redistribute it and/or modify
+% it under the terms of the GNU General Public License version 3 as
+% published by the Free Software Foundation.
+% 
+% PbDlib is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU General Public License for more details.
+% 
+% You should have received a copy of the GNU General Public License
+% along with PbDlib. If not, see <https://www.gnu.org/licenses/>.
 
 addpath('./m_fcts/');