diff --git a/doc/rcfs.tex b/doc/rcfs.tex index ead09218242cea1f12bea9bce2e7f1d1c0a435c5..1b3ff2b6a114db1d1351f517664ba8ba48167543 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 4ff515a1c9c81f2d22ae1f249960a77db6a00105..5e82a9e36a2fe735204d7707d4a73361790e5ad2 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