diff --git a/doc/rcfs.pdf b/doc/rcfs.pdf
index 3549eb61368a91f8e27655b1186a4893bd7c65e8..5ac075e3c1a39d5ff18711888db9c7c1ce28d244 100644
Binary files a/doc/rcfs.pdf and b/doc/rcfs.pdf differ
diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index a8af112019c488a5363655f4102bdff6679ab0ee..2c1ab34625f65dca04d9a350e54c9180b2bbecf7 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -2,6 +2,7 @@
\documentclass[10pt,a4paper]{article} %twocolumn
\usepackage{graphicx,amsmath,amssymb,bm,xcolor,soul,nicefrac,listings,algorithm2e,dsfont,caption,subcaption,wrapfig,sidecap}
\usepackage[hidelinks]{hyperref}
+\usepackage[makeroom]{cancel}
%pseudocode
\newcommand\mycommfont[1]{\footnotesize\ttfamily\textcolor{lightgray}{#1}}
@@ -343,7 +344,7 @@ where $\bm{J}_{\bm{r}}\in\mathbb{R}^{R\times D}$ is the Jacobian matrix of $\bm{
The update rule in \eqref{eq:Taylor_nD_update} then becomes
\begin{align}
\bm{x}_{k+1} &= \bm{x}_k - {\big(\bm{J}_{\bm{r}}^\trsp(\bm{x}_k) \bm{J}_{\bm{r}}(\bm{x}_k)\big)}^{-1} \,
- \bm{J}_{\bm{r}}^\trsp(\bm{x}_k) \, \bm{r}(\bm{x}_k) \\
+ \bm{J}_{\bm{r}}^\trsp(\bm{x}_k) \, \bm{r}(\bm{x}_k) \nonumber\\
&= \bm{x}_k - \bm{J}_{\bm{r}}^\psin(\bm{x}_k) \, \bm{r}(\bm{x}_k),
\label{eq:GaussNewtonUpdate}
\end{align}
@@ -356,16 +357,62 @@ The Gauss--Newton algorithm is the workhorse of many robotics problems, includin
\subsection{Least squares}\label{sec:LS}
When the cost $c(\bm{x})$ is a quadratic function w.r.t.\ $\bm{x}$, the optimization problem can be solved directly, without requiring iterative steps. Indeed, for any matrix $\bm{A}$ and vector $\bm{b}$, we can see that if
-\begin{equation*}
- c(\bm{x}) = (\bm{A}\bm{x}-\bm{b})^\trsp (\bm{A}\bm{x}-\bm{b}),
-\end{equation*}
-derivating $c(\bm{x})$ w.r.t.\ $\bm{x}$ and equating to zero yields
-\begin{equation*}
+\begin{equation}
+ c(\bm{x}) = \|\bm{A}\bm{x}-\bm{b}\|^2 = (\bm{A}\bm{x}-\bm{b})^\trsp (\bm{A}\bm{x}-\bm{b}),
+ \label{eq:costquadratic}
+\end{equation}
+deriving $c(\bm{x})$ w.r.t.\ $\bm{x}$ and equating to zero yields
+\begin{equation}
\bm{A}\bm{x} - \bm{b} = \bm{0}
- \iff \bm{x} = \bm{A}^\psin\bm{b},
-\end{equation*}
+ \iff \bm{x} = (\bm{A}^\trsp \bm{A})^{-1} \bm{A}^\trsp \bm{b} = \bm{A}^\psin \bm{b},
+ \label{eq:costquadraticsolution}
+\end{equation}
which corresponds to the standard analytic least squares estimate. We will see later in the inverse kinematics section that for the complete solution can also include a nullspace structure.
+In contrast to the Gauss--Newton algorithm presented in Section \ref{sec:GaussNewton}, the optimization problem in \eqref{eq:costquadratic}, described by a quadratic cost on the decision variable $\bm{x}$, admits the solution \eqref{eq:costquadraticsolution} that can be computed directly without relying on an iterative procedure. We can observe that if we follow the Gauss--Newton procedure, the residual vector corresponding to the cost \eqref{eq:costquadratic} is $\bm{r}=\bm{A}\bm{x}-\bm{b}$ and its Jacobian matrix is $\bm{J}_{\bm{r}}=\bm{A}$. By starting from an initial estimate $\bm{x}_0$, the Gauss--Newton update in \eqref{eq:GaussNewtonUpdate} then takes the form
+\begin{align*}
+ \bm{x}_{k+1} &= \bm{x}_k - \bm{A}^\psin (\bm{A}\bm{x}_k-\bm{b})\\
+ &= \bm{x}_k - (\bm{A}^\trsp \bm{A})^{-1} \bm{A}^\trsp (\bm{A}\bm{x}_k-\bm{b})\\
+ &= \bm{x}_k - \cancel{(\bm{A}^\trsp \bm{A})^{-1}} \cancel{(\bm{A}^\trsp\bm{A})} \bm{x}_k + \bm{A}^\psin \bm{b}\\
+ &= \bm{A}^\psin \bm{b},
+\end{align*}
+which converges in a single iteration, independently of the initial guess $\bm{x}_0$. Indeed, a cost that takes a quadratic form with respect to the decision variable can be soled in batch form (least squares solution), which will be a useful property that we will exploit later in the context of linear quadratic controllers.
+
+A ridge regression problem can similarly be defined as
+\begin{align}
+ \bm{\hat{x}} &= \arg\min_{\bm{x}} \; \|\bm{A}\bm{x}-\bm{b})\|^2 + \alpha \|\bm{x}\|^2 \nonumber\\
+ &= (\bm{A}^\trsp \bm{A} + \alpha\bm{I})^{-1} \bm{A}^\trsp \bm{b}, \label{eq:regLS}
+\end{align}
+which is also called penalized least squares, robust regression, damped least squares or Tikhonov regularization. In \eqref{eq:regLS}, $\bm{I}$ denotes an identity matrix (diagonal matrix with 1 as elements in the diagonal). $\alpha$ is typically a small scalar value acting as a regularization term when inverting the matrix $\bm{A}^\trsp \bm{A}$.
+
+The cost can also be weighted by a matrix $\bm{W}$, providing the weighted least squares solution
+\begin{align}
+ \bm{\hat{x}} &= \arg\min_{\bm{x}} \; \|\bm{A}\bm{x}-\bm{b}\|^2_{\bm{W}} \nonumber\\
+ &= \arg\min_{\bm{x}} \; (\bm{A}\bm{x}-\bm{b})^\trsp \bm{W} (\bm{A}\bm{x}-\bm{b}) \nonumber\\
+ &= (\bm{A}^\trsp \bm{W} \bm{A})^{-1} \bm{A}^\trsp \bm{W} \bm{b}. \label{eq:WLS}
+\end{align}
+
+By combining \eqref{eq:regLS} and \eqref{eq:WLS}, a weighted ridge regression problem can be defined as
+\begin{align}
+ \bm{\hat{x}} &= \arg\min_{\bm{x}} \; \|\bm{A}\bm{x}-\bm{b}\|^2_{\bm{W}} + \alpha \|\bm{x}\|^2 \nonumber\\
+ &= (\bm{A}^\trsp \bm{W} \bm{A} + \alpha\bm{I})^{-1} \bm{A}^\trsp \bm{W} \bm{b}. \label{eq:regWLS}
+\end{align}
+
+If the matrices and vectors in \eqref{eq:regWLS} have the structure
+\begin{equation*}
+ \bm{W}=\begin{bmatrix}\bm{W}_{1}&\bm{0}\\\bm{0}&\bm{W}_{2}\end{bmatrix},\quad
+ \bm{A}=\begin{bmatrix}\bm{A}_{1}\\\bm{A}_{2}\end{bmatrix},\quad
+ \bm{x}=\begin{bmatrix}\bm{x}_{1}\\\bm{x}_{2}\end{bmatrix},\quad
+ \bm{b}=\begin{bmatrix}\bm{b}_{1}\\\bm{b}_{2}\end{bmatrix},
+\end{equation*}
+we could consider an optimization problem using only the first part of the matrices, which would yield the estimate
+\begin{align}
+ \bm{\hat{x}} &= \arg\min_{\bm{x}} \; \|\bm{A}_1\bm{x}-\bm{b}_1\|^2_{\bm{W}_1} + \alpha \|\bm{x}\|^2 \nonumber\\
+ &= (\bm{A}_1^\trsp \bm{W}_1 \bm{A}_1 + \alpha\bm{I})^{-1} \bm{A}_1^\trsp \bm{W}_1 \bm{b}_1. \label{eq:regWLS1}
+\end{align}
+
+It is important to observe that the result in \eqref{eq:regWLS1} is not the same as that of computing \eqref{eq:regWLS} with $\bm{W}_2=\bm{0}$.
+
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\subsection{Least squares with constraints}\label{sec:LSconstraints}
diff --git a/matlab/spline1D.m b/matlab/spline1D.m
index f822cbeec5ea200b20317de732ccb295af01dc43..e20e2eee8c1487d2c81bb43c43f9819193567e41 100644
--- a/matlab/spline1D.m
+++ b/matlab/spline1D.m
@@ -15,7 +15,7 @@ param.nbFct = 4; %Number of basis functions (code valid for param.nbFct>2)
param.nbSeg = 7; %Number of segments
param.nbIn = 1; %Dimension of input data (here: time)
param.nbOut = 1; %Dimension of position data (here: x)
-param.nbDim = param.nbFct*param.nbSeg*20; %Number of datapoints in a trajectory
+param.nbDim = param.nbFct*param.nbSeg*20+1; %Number of datapoints in a trajectory
%% Load handwriting data
@@ -70,7 +70,7 @@ end
param.BC = param.B * param.C;
%Time parameters matrix to compute positions
-t = linspace(0, 1-1/param.nbDim, param.nbDim);
+t = linspace(0, 1, param.nbDim);
delta_t = t(2) - t(1);
[Psi, dPsi, phi] = computePsiList(t, param);
@@ -78,18 +78,18 @@ delta_t = t(2) - t(1);
%% Trajectory encoding and reproduction as movement primitives
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Batch estimation of superposition weights from reference position profile
-wb = Psi \ x0;
+w = Psi \ x0;
%%Batch estimation of superposition weights from reference position profile with additional cost on derivatives
-%wb = (Psi'*Psi + dPsi'*dPsi*1E-3) \ Psi' * x0;
+%w = (Psi'*Psi + dPsi'*dPsi*1E-3) \ Psi' * x0;
-param.Mw = param.BC * wb; %Transformation matrix
+param.Mw = param.BC * w; %Transformation matrix
%Reconstruction of trajectories
-xb = Psi * wb;
-dxb = dPsi * wb;
+x = Psi * w;
+dx = dPsi * w;
%Useful only for visualization
-wb_vis = kron(param.C, eye(param.nbOut)) * wb;
+w_vis = kron(param.C, eye(param.nbOut)) * w;
%% Plots
@@ -101,7 +101,7 @@ clrmap = lines(param.nbFct * param.nbSeg); %Colors for total number of control p
%Plot reference
subplot(3,2,1); hold on; title('Reference position profile (in red)');
plot(t, x0, 'linewidth',4,'color',[.8 .6 .6]);
-plot(t, xb, 'linewidth',4,'color',[.6 .6 .6]);
+plot(t, x, 'linewidth',4,'color',[.6 .6 .6]);
xlabel('t','fontsize',30);
ylabel('x','fontsize',30);
set(gca,'xtick',[],'ytick',[]);
@@ -109,7 +109,7 @@ set(gca,'xtick',[],'ytick',[]);
%Plot derivatives
subplot(3,2,2); hold on; title('Velocity profile');
plot(t(1:end-1), diff(x0)/delta_t, 'linewidth',8,'color',[.8 .6 .6]);
-plot(t, dxb, 'linewidth',4,'color',[.6 .6 .6]);
+plot(t, dx, 'linewidth',4,'color',[.6 .6 .6]);
axis tight;
xlabel('t','fontsize',30);
ylabel('dx/dt','fontsize',30);
@@ -117,21 +117,21 @@ set(gca,'xtick',[],'ytick',[]);
subplot(3,2,3); hold on; title('Reconstructed position profile');
%Plot reconstructed signal
-plot(t, xb, 'linewidth',4,'color',[.6 .6 .6]);
+plot(t, x, 'linewidth',4,'color',[.6 .6 .6]);
%Plot control points
tSeg = [];
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, wb_vis, '.', 'markersize',38,'color',[0 0 0]);
+plot(tSeg, w_vis, '.', 'markersize',38,'color',[0 0 0]);
%Plot segments between control points
for n=1:param.nbSeg
id = (n-1)*param.nbFct + [1,2];
- plot(tSeg(id), wb_vis(id), '-', 'linewidth',2,'color',[0 0 0]);
+ plot(tSeg(id), w_vis(id), '-', 'linewidth',2,'color',[0 0 0]);
id = n*param.nbFct + [-1,0];
- plot(tSeg(id), wb_vis(id), '-', 'linewidth',2,'color',[0 0 0]);
- plot([tSeg(id(2)), tSeg(id(2))], [min(wb)-2, max(wb)+2], '-', 'linewidth',2,'color',[.8 .8 .8]);
+ plot(tSeg(id), w_vis(id), '-', 'linewidth',2,'color',[0 0 0]);
+ plot([tSeg(id(2)), tSeg(id(2))], [min(w)-2, max(w)+2], '-', 'linewidth',2,'color',[.8 .8 .8]);
end
axis tight;
xlabel('t','fontsize',30);
@@ -140,12 +140,12 @@ set(gca,'xtick',[],'ytick',[]);
%Plot reconstructed derivatives
subplot(3,2,4); hold on; title('Reconstructed velocity profile');
-plot(t, dxb,'linewidth',4,'color',[.6 .6 .6]);
-%plot(t(1:end-1), diff(xb)/delta_t, ':','linewidth',4,'color',[.2 .2 .2]);
+plot(t, dx,'linewidth',4,'color',[.6 .6 .6]);
+%plot(t(1:end-1), diff(x)/delta_t, ':','linewidth',4,'color',[.2 .2 .2]);
%Plot segments between control points
for n=1:param.nbSeg
id = n*param.nbFct + [-1,0];
- plot([tSeg(id(2)), tSeg(id(2))], [min(dxb)-2, max(dxb)+2], '-', 'linewidth',2,'color',[.8 .8 .8]);
+ plot([tSeg(id(2)), tSeg(id(2))], [min(dx)-2, max(dx)+2], '-', 'linewidth',2,'color',[.8 .8 .8]);
end
axis tight;
xlabel('t','fontsize',30);