diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 4d76ac6b3c3192e3a958cfd3a24024d32293b06a..dae5823cb4115b16f22e4e743723852df635bd4c 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 73a7f761e467151181c579692940e98de9941067..945009c25e0683330dc455da47291d5525dda5e2 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -2792,6 +2792,55 @@ where $\alpha$ is a line search parameter, $\bm{r}$ is a vector concatenating ve
 Figure \ref{fig:iLQR_decoupling} presents a simple example within a 2D reaching problem with a point mass agent and velocity commands.
 
 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+\subsubsection{Curvature}
+
+\begin{wrapfigure}{r}{.38\textwidth}
+%\vspace{-20pt}
+\centering
+\includegraphics[width=.32\textwidth]{images/iLQR_curvature01.png}
+\caption{\footnotesize
+Point mass control problem starting form an initial point (in black), with a cost asking to pass through a set of viapoints (in red), together with a cost on curvature (black path).
+}
+\label{fig:curvature}
+\end{wrapfigure}
+
+The curvature of a 2-dimensional curve is defined as
+\begin{equation}
+	\kappa = \frac{\dot{\text{x}}_1\ddot{\text{x}}_2-\dot{\text{x}}_2\ddot{\text{x}}_1}
+	{(\dot{\text{x}}_1^2+\dot{\text{x}}_2^2)^\frac{3}{2}}.
+	\label{eq:curvature0}
+\end{equation}
+
+We describe the system state in vector form as
+\begin{equation}
+	\bm{x} = \begin{bmatrix} \mathbf{x} \\ \mathbf{\dot{x}} \\ \mathbf{\ddot{x}} \end{bmatrix}.
+\end{equation}
+
+By defining a set of selection vectors $\bm{s}_{i,j}$ so that $\dot{\text{x}}_1 = \bm{s}_{1,1}^\trsp \bm{x}, \; \dot{\text{x}}_2 = \bm{s}_{1,2}^\trsp \bm{x}, \; \ddot{\text{x}}_1 = \bm{s}_{2,1}^\trsp \bm{x}, \; \ddot{\text{x}}_2 = \bm{s}_{2,2}^\trsp \bm{x}$, we can observe that
+\begin{equation}
+	\dot{\text{x}}_1 \ddot{\text{x}}_2 = (\bm{s}_{1,1}^\trsp \bm{x}) (\bm{s}_{2,2}^\trsp \bm{x}) = \bm{x}^\trsp (\bm{s}_{1,1} \bm{s}_{2,2}^\trsp) \bm{x},
+%	= \bm{x}^\trsp \bm{S}_{1,1}^{2,2} \bm{x},
+\end{equation}
+where $(\bm{s}_{1,1} \bm{s}_{2,2}^\trsp)$ is a selection matrix.
+
+By leveraging this matrix formulation, the curvature in \eqref{eq:curvature0} can be expressed as
+\begin{equation}
+	%\kappa(\bm{x}) = \begin{frac}{\bm{x}^\trsp \bm{S}_A \bm{x}}{(\bm{x}^\trsp \bm{S}_B \bm{x})^{\frac{3}{2}}},
+	\kappa(\bm{x}) = (\bm{x}^\trsp \bm{S}_B \bm{x})^{-\frac{3}{2}} \; \bm{x}^\trsp \bm{S}_A \bm{x},
+	\label{eq:curvature}
+\end{equation}
+with selection matrices $\bm{S}_A=\bm{s}_{1,1} \bm{s}_{2,2}^\trsp - \bm{s}_{1,2} \bm{s}_{2,1}^\trsp$ and $\bm{S}_B=\bm{s}_{1,1} \bm{s}_{1,1}^\trsp + \bm{s}_{1,2} \bm{s}_{1,2}^\trsp$.
+
+With this formulation, by using the derivative property $(fg)'=f'g+fg'$, the derivatives of \eqref{eq:curvature0} w.r.t $\bm{x}$ form the Jacobian
+\begin{equation}
+	\bm{J}(\bm{x}) = \frac{\partial \kappa(\bm{x})}{\partial\bm{x}} = 
+	2 \; (\bm{x}^\trsp \bm{S}_B \bm{x})^{-\frac{3}{2}} \; \bm{S}_A \bm{x}  -
+	3 \; (\bm{x}^\trsp \bm{S}_A \bm{x}) \; (\bm{x}^\trsp \bm{S}_B \bm{x})^{-\frac{5}{2}} \; \bm{S}_B \bm{x}. 
+	\label{eq:diff_curvature}
+\end{equation}
+
+
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 \subsection{iLQR with control primitives}
 \begin{flushright}