diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index fbe02741ad0a28c80f3b2d84db23504a39b3d5ab..aeea9767195766c273a6069ed610f78e0597faa5 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index 1b3ff2b6a114db1d1351f517664ba8ba48167543..90ff864ac96e440ceae80cf0c76776c5246fad16 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -896,12 +896,12 @@ By using basis functions as analytic expressions, the derivatives are easy to co
\end{equation}
providing the derivatives of $\bm{\Psi}(\bm{t})$ with respect to $\bm{t}$ expressed as
\begin{equation}
- \bm{\nabla}\bm{\Psi}(\bm{t}) = \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_1} \,\otimes\, \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_2},
+ \bm{\nabla}\!\bm{\Psi}(\bm{t}) = \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_1} \,\otimes\, \frac{\partial\bm{\Psi}(\bm{t})}{\partial t_2},
\label{eq:dPsi}
\end{equation}
which can be used to compute the 2D gradient of the SDF at location $\bm{t}$ with
\begin{equation}
- \bm{\nabla}x = \bm{\nabla}\bm{\Psi}(\bm{t}) \, \bm{w}.
+ \bm{\nabla}x = \bm{\nabla}\!\bm{\Psi}(\bm{t}) \, \bm{w}.
\end{equation}
When using splines of the form $\bm{\phi}(t)=\bm{T}(t)\bm{B}\bm{C}$, the derivatives in \eqref{eq:parialPsi} are simply computed as $\frac{\partial\bm{\phi}(t)}{\partial t}=\frac{\partial\bm{T}(t)}{\partial t} \bm{B}\bm{C}$. For a cubic splines, it corresponds to $\bm{T}(t) = [1,t,t^2,t^3]$ and $\frac{\partial\bm{T}(t)}{\partial t} = [0,1,2t,3t^2]$.\newline
@@ -1001,16 +1001,23 @@ which ensures that $w_4=w_5$ and $w_6=-w_3+2w_5$. These constraints guarantee th
\subsection{Batch computation of basis functions coefficients}
\label{sec:batchSDF}
-Based on observed data $\bm{x}$, the superposition weights $\bm{w}$ can be estimated as a simple least squares estimate
+Based on observed data $\bm{x}$, the superposition weights $\bm{\hat{w}}$ can be estimated as a simple least squares estimate
\begin{equation}
- \bm{w} = \bm{\Psi}^\psin \bm{x},
+ \bm{\hat{w}} = \bm{\Psi}^\psin \bm{x} = {(\bm{\Psi}^\trsp\bm{\Psi}}^{-1} \bm{\Psi}^\trsp \bm{x},
\end{equation}
or as the regularized version (ridge regression)
\begin{equation}
- \bm{w} = {(\bm{\Psi}^\trsp\bm{\Psi} + \lambda\bm{I})}^{-1} \bm{\Psi}^\trsp \bm{x}.
+ \bm{\hat{w}} = {(\bm{\Psi}^\trsp\bm{\Psi} + \lambda\bm{I})}^{-1} \bm{\Psi}^\trsp \bm{x}.
\label{eq:ridge}
\end{equation}
+For example, if we want to fit a reference path, which can also be sparse or composed of a set of viapoints, while minimizing velocities (or similarly, any other derivatives, such as computing a minimum jerk trajectories), we can solve
+\begin{align}
+ \bm{\hat{w}} &= \arg\min_{\bm{w}} \|\bm{\Psi}\bm{w} - \bm{x}\|^2 + \lambda \|\bm{\nabla}\!\bm{\Psi}\bm{w}\|^2 \\
+ &= {(\bm{\Psi}^\trsp\bm{\Psi} + \lambda\bm{\nabla}\!\bm{\Psi}^\trsp\bm{\nabla}\!\bm{\Psi})}^{-1} \bm{\Psi}^\trsp \bm{x}.
+ \label{eq:ridge2}
+\end{align}
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Recursive computation of basis functions coefficients}
@@ -3014,6 +3021,25 @@ which can then be converted to a discrete form with \eqref{eq:AdAc}.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\section{Ergodic control}\label{sec:ergodicControl}
+Ergodic theory studies the connection between the time-averaged and space-averaged behaviors of a dynamical system. By using it in a search and exploration context, it enables optimal exploration of an information distribution. Exploration problems can be formulated in various ways, see Figures \ref{fig:ergodic_intro} and \ref{fig:ergodic_vs_patterned}.
+
+Research in ergodic control addresses fundamental challenges linking machine learning, optimal control, signal processing and information theory. A conventional tracking problem in robotics is characterized by a target to reach, requiring a controller to be computed to reach this target. In \emph{ergodic control}, instead of providing a single target point, a probability distribution is given to the robot, which must cover the distribution in an efficient way. Ergodic control thus consists of moving within a spatial distribution by spending time in each part of the distribution in proportion to its density.
+%(namely, ``tracking a distribution'' instead of ``tracking a point'').
+The term ergodicity corresponds here to the difference between the time-averaged spatial statistics of the agent's trajectory and the target distribution to search in. By minimizing this difference, the resulting controller generates natural exploration behaviors.
+
+In robotics, ergodic control can be exploited in a wide range of problems requiring the automatic exploration of regions of interest, which can be used in a wide range of tasks, including active sensing, localization, surveillance, or insertion (see Figure \ref{fig:ergodic_insertion} for an example). This is particularly helpful when the available sensing information or robot actuators are not accurate enough to fulfill the task with a standard controller (limited vision, soft robotic manipulator, etc.), but where this information can still guide the robot towards promising areas. In a collaborative task, it can also be used when the operator's input is not accurate enough to fully reproduce the task, which then requires the robot to explore around the requested input (e.g., a point of interest selected by the operator). For picking and insertion problems, ergodic control can be applied to move around the picking/insertion point, thereby facilitating the prehension/insertion. It can also be employed for active sensing and localization (either detected autonomously, or with help by the operator). Here, the robot can plan movements based on the current information density, and can recompute the commands when new measurements are available (i.e., updating the spatial distribution used as target).
+
+Ergodic control is originally formulated as a \emph{spectral multiscale coverage} (SMC) objective \cite{Mathew09}, which we will see next. Other ergodic control techniques have later been proposed, including the \emph{heat equation driven area coverage} (HEDAC) \cite{Ivic17}, which we will also be introduced next.
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+\subsection{Spectral-based ergodic control (SMC)}\label{sec:SMC}
+
+The underlying objective of \emph{spectral multiscale coverage} (SMC) takes a simple form, corresponding to a tracking problem in the spectral domain (matching of frequency components) \cite{Mathew09}. The advantage of such a simple control formulation is that it can be easily combined with other control objectives and constraints.
+
+It requires the spatial distribution to be decomposed as Fourier series, with a cost function comparing the spectral decomposition of the robot path with the spectral decomposition of the distribution, in the form of a weighted distance function with a decreasing weight from low frequency to high frequency components. The resulting controller allows the robot to explore the given spatial distribution in a natural manner, \textbf{by starting from a crude exploration and by refining the search progressively} (i.e., matching the Fourier coefficients with an increasing importance from low to high frequency components).
+
+
\begin{figure}
\centering{\includegraphics[width=.6\columnwidth]{images/ergodicVsPatternedSearch01.jpg}}
\caption{\footnotesize
@@ -3055,24 +3081,6 @@ Insertion tasks can be achieved robustly with ergodic control, by not only relyi
}
\end{figure}
-Ergodic theory studies the connection between the time-averaged and space-averaged behaviors of a dynamical system. By using it in a search and exploration context, it enables optimal exploration of an information distribution. Exploration problems can be formulated in various ways, see Figures \ref{fig:ergodic_intro} and \ref{fig:ergodic_vs_patterned}.
-
-Research in ergodic control addresses fundamental challenges linking machine learning, optimal control, signal processing and information theory. A conventional tracking problem in robotics is characterized by a target to reach, requiring a controller to be computed to reach this target. In \emph{ergodic control}, instead of providing a single target point, a probability distribution is given to the robot, which must cover the distribution in an efficient way. Ergodic control thus consists of moving within a spatial distribution by spending time in each part of the distribution in proportion to its density.
-%(namely, ``tracking a distribution'' instead of ``tracking a point'').
-The term ergodicity corresponds here to the difference between the time-averaged spatial statistics of the agent's trajectory and the target distribution to search in. By minimizing this difference, the resulting controller generates natural exploration behaviors.
-
-In robotics, ergodic control can be exploited in a wide range of problems requiring the automatic exploration of regions of interest, which can be used in a wide range of tasks, including active sensing, localization, surveillance, or insertion (see Figure \ref{fig:ergodic_insertion} for an example). This is particularly helpful when the available sensing information or robot actuators are not accurate enough to fulfill the task with a standard controller (limited vision, soft robotic manipulator, etc.), but where this information can still guide the robot towards promising areas. In a collaborative task, it can also be used when the operator's input is not accurate enough to fully reproduce the task, which then requires the robot to explore around the requested input (e.g., a point of interest selected by the operator). For picking and insertion problems, ergodic control can be applied to move around the picking/insertion point, thereby facilitating the prehension/insertion. It can also be employed for active sensing and localization (either detected autonomously, or with help by the operator). Here, the robot can plan movements based on the current information density, and can recompute the commands when new measurements are available (i.e., updating the spatial distribution used as target).
-
-Ergodic control is originally formulated as a \emph{spectral multiscale coverage} (SMC) objective \cite{Mathew09}, which we will see next. Other ergodic control techniques have later been proposed, including the \emph{heat equation driven area coverage} (HEDAC) \cite{Ivic17}, which we will also be introduced next.
-
-
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-\subsection{Spectral-based ergodic control (SMC)}\label{sec:SMC}
-
-The underlying objective of \emph{spectral multiscale coverage} (SMC) takes a simple form, corresponding to a tracking problem in the spectral domain (matching of frequency components) \cite{Mathew09}. The advantage of such a simple control formulation is that it can be easily combined with other control objectives and constraints.
-
-It requires the spatial distribution to be decomposed as Fourier series, with a cost function comparing the spectral decomposition of the robot path with the spectral decomposition of the distribution, in the form of a weighted distance function with a decreasing weight from low frequency to high frequency components. The resulting controller allows the robot to explore the given spatial distribution in a natural manner, \textbf{by starting from a crude exploration and by refining the search progressively} (i.e., matching the Fourier coefficients with an increasing importance from low to high frequency components).
-
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsubsection{Unidimensional SMC}\label{sec:SMC1D}
diff --git a/matlab/spline1D.m b/matlab/spline1D.m
index 20425d9661fd51d463ff0c474712fba9c6d00139..f822cbeec5ea200b20317de732ccb295af01dc43 100644
--- a/matlab/spline1D.m
+++ b/matlab/spline1D.m
@@ -40,7 +40,7 @@ param.B = kron(eye(param.nbSeg), param.B0); %Transform to multidimensional basis
%and the two control points around should be symmetric (w3-2*w5+w6=0, ...)
if param.nbFct==3
C0 = [1; 1; 2];
- param.C = blkdiag(eye(param.nbFct-1));
+ param.C = eye(param.nbFct-1);
for n=1:param.nbSeg-1
param.C = blkdiag(param.C, C0);
end