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