From 6d8cf01eb7bbc51d513265ca1e32e6de3e2816c0 Mon Sep 17 00:00:00 2001
From: Sylvain CALINON <sylvain.calinon@idiap.ch>
Date: Tue, 6 Aug 2024 14:50:02 +0200
Subject: [PATCH] spline refined

---
 doc/rcfs.tex      |  2 +-
 matlab/spline1D.m | 14 +++++++++-----
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/doc/rcfs.tex b/doc/rcfs.tex
index ead0921..1b3ff2b 100644
--- a/doc/rcfs.tex
+++ b/doc/rcfs.tex
@@ -133,7 +133,7 @@ The solution of a quadratic cost function can be viewed probabilistically as cor
 	&= \|\bm{x}-\bm{\mu}\|_{\bm{W}}^2,
 	\label{eq:quadratic}
 \end{align}
-has an optimal solution $\bm{x}^* = \bm{\mu}$. This solution does not contain much information about the cost function itself. Alternatively, we can view $\bm{x}$ as a random variable with a Gaussian distribution, i.e., $p(\bm{x}) = \mathcal{N}(\bm{\mu}, \bm{\Sigma})$ where $\bm{\mu}$ and $\bm{\Sigma}=\bm{W}^{-1}$ are the mean vector and covariance matrix of the Gaussian, respectively. The negative log-likelihood of this Gaussian distribution is equivalent to \eqref{eq:quadratic} up to a constant factor. According to $p(\bm{x})$, $\bm{x}$ has the highest probability at $\bar{\bm{x}}$, and $\bm{\Sigma}$ gives the directional information on how this probability changes as we move away from $\bm{\mu}$. The point having the lowest cost in \eqref{eq:quadratic} is therefore associated with the point having the highest probability. 
+has an optimal solution $\bm{x}^* = \bm{\mu}$. This solution does not contain much information about the cost function itself. Alternatively, we can view $\bm{x}$ as a random variable with a Gaussian distribution, i.e., $p(\bm{x}) = \mathcal{N}(\bm{\mu}, \bm{\Sigma})$, where $\bm{\mu}$ and $\bm{\Sigma}=\bm{W}^{-1}$ are the mean vector and covariance matrix of the Gaussian, respectively. The negative log-likelihood of this Gaussian distribution is equivalent to \eqref{eq:quadratic} up to a constant factor. According to $p(\bm{x})$, $\bm{x}$ has the highest probability at $\bar{\bm{x}}$, and $\bm{\Sigma}$ gives the directional information on how this probability changes as we move away from $\bm{\mu}$. The point having the lowest cost in \eqref{eq:quadratic} is therefore associated with the point having the highest probability. 
 
 Similarly, the solution of a cost function composed of several quadratic terms 
 \begin{equation}
diff --git a/matlab/spline1D.m b/matlab/spline1D.m
index 4ff515a..5e82a9e 100644
--- a/matlab/spline1D.m
+++ b/matlab/spline1D.m
@@ -77,8 +77,12 @@ delta_t = t(2) - t(1);
 
 %% Trajectory encoding and reproduction as movement primitives
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%Batch estimation of superposition weights from permuted data
+%Batch estimation of superposition weights from reference position profile
 wb = Psi \ x0;
+
+%%Batch estimation of superposition weights from reference position profile with additional cost on derivatives
+%wb = (Psi'*Psi + dPsi'*dPsi*1E-6) \ Psi' * x0;
+
 param.Mw = param.BC * wb; %Transformation matrix
 
 %Reconstruction of trajectories
@@ -95,7 +99,7 @@ set(gca,'linewidth',2);
 clrmap = lines(param.nbFct * param.nbSeg); %Colors for total number of control points 
 
 %Plot reference
-subplot(3,2,1); hold on; title('Reference position'); 
+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]);
 xlabel('t','fontsize',30); 
@@ -103,7 +107,7 @@ ylabel('x','fontsize',30);
 set(gca,'xtick',[],'ytick',[]);
 
 %Plot derivatives
-subplot(3,2,2); hold on; title('Reference velocity'); 
+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]);
 axis tight;
@@ -111,7 +115,7 @@ xlabel('t','fontsize',30);
 ylabel('dx/dt','fontsize',30);
 set(gca,'xtick',[],'ytick',[]);
 
-subplot(3,2,3); hold on; title('Reconstructed position'); 
+subplot(3,2,3); hold on; title('Reconstructed position profile'); 
 %Plot reconstructed signal
 plot(t, xb, 'linewidth',4,'color',[.6 .6 .6]);
 %Plot control points
@@ -135,7 +139,7 @@ ylabel('x','fontsize',30);
 set(gca,'xtick',[],'ytick',[]);
 
 %Plot reconstructed derivatives
-subplot(3,2,4); hold on; title('Reconstructed velocity'); 
+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 segments between control points
-- 
GitLab