diff --git a/README.md b/README.md index c2f336fbcba8d646d67ede031e308b4a7b7782ad..b4902d9fb39c7ec316a14eda94645003d2adf552 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ Examples starting with `demo_` can be run as examples. The corresponding publica ### List of examples -All the examples are located in the main folder, and the functions are located in the `m_fcts` folder. +All the examples are located in the `demos` folder, and the functions are located in the `m_fcts` folder. | Filename | Ref. | Description | |----------|------|-------------| @@ -121,6 +121,7 @@ All the examples are located in the main folder, and the functions are located i | [demo_IK_weighted01.m](./demos/demo_IK_weighted01.m) | [[10]](#ref-10) | Inverse kinematics with nullspace control, by considering weights in joint space and in task space | | [demo_ILC01.m](./demos/demo_ILC01.m) | [[10]](#ref-10) | Iterative correction of errors for a recurring movement with ILC | | [demo_IPRA01.m](./demos/demo_IPRA01.m) | [[10]](#ref-10) | Gaussian mixture model (GMM) learned with iterative pairwise replacement algorithm (IPRA) | +| [demo_Kalman01.m](./demos/demo_Kalman01.m) | [[10]](#10) | Kalman filter computed as a feedback term or as a product of Gaussians | | [demo_kernelPCA01.m](./demos/demo_kernelPCA01.m) | [[10]](#ref-10) | Kernel PCA, with comparison to PCA | | [demo_LS01.m](./demos/demo_LS01.m) | [[10]](#ref-10) | Multivariate ordinary least squares | | [demo_LS_IRLS01.m](./demos/demo_LS_IRLS01.m) | [[10]](#ref-10) | Iteratively reweighted least squares | @@ -257,7 +258,7 @@ All the examples are located in the main folder, and the functions are located i ### References -If you find PbDLib useful for your research, please cite the related publications! +If you find PbDlib useful for your research, please cite the related publications! <p><a name="ref-1"> [1] Calinon, S. (2016). <strong>A Tutorial on Task-Parameterized Movement Learning and Retrieval</strong>. Intelligent Service Robotics (Springer), 9:1, 1-29. @@ -268,7 +269,7 @@ If you find PbDLib useful for your research, please cite the related publication </p> <p><a name="ref-2"> -[2] Calinon, S. and Lee, D. (2019). <strong>Learning Control</strong>. Vadakkepat, P. and Goswami, A. (eds.). Humanoid Robotics: a Reference, pp. 1261-1312. Springer. +[2] Calinon, S. and Lee, D. (2019). <strong>Learning Control</strong>. Vadakkepat, P. and Goswami, A. (eds.). Humanoid Robotics: a Reference, pp. 1261-1312. Springer. </a><br> [[pdf]](http://calinon.ch/papers/Calinon-Lee-learningControl.pdf) [[bib]](http://calinon.ch/papers/Calinon-Lee-learningControl.bib) @@ -284,7 +285,7 @@ If you find PbDLib useful for your research, please cite the related publication </p> <p><a name="ref-4"> -[4] Jaquier, N. and Calinon, S. (2017). <strong>Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with sEMG</strong>. In Proc. of the IEEE/RSJ Intl Conf. on Intelligent Robots and Systems (IROS), pp. 59-64. +[4] Jaquier, N. and Calinon, S. (2017). <strong>Gaussian Mixture Regression on Symmetric Positive Definite Matrices Manifolds: Application to Wrist Motion Estimation with sEMG</strong>. In Proc. of the IEEE/RSJ Intl Conf. on Intelligent Robots and Systems (IROS), pp. 59-64. </a><br> [[pdf]](http://calinon.ch/papers/Jaquier-IROS2017.pdf) [[bib]](http://calinon.ch/papers/Jaquier-IROS2017.bib) diff --git a/demos/demo_Kalman01.m b/demos/demo_Kalman01.m new file mode 100644 index 0000000000000000000000000000000000000000..7e8c25f64bc7ed0c7bcb740168725d3d216c486e --- /dev/null +++ b/demos/demo_Kalman01.m @@ -0,0 +1,176 @@ +function demo_Kalman01 +% Kalman filter computed as a feedback term or as a product of Gaussians. +% +% If this code is useful for your research, please cite the related publication: +% @misc{pbdlib, +% title = {{PbDlib} robot programming by demonstration software library}, +% howpublished = {\url{http://www.idiap.ch/software/pbdlib/}}, +% note = {Accessed: 2019/04/18} +% } +% +% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/ +% Written by Sylvain Calinon, http://calinon.ch/ +% +% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/ +% +% PbDlib is free software: you can redistribute it and/or modify +% it under the terms of the GNU General Public License version 3 as +% published by the Free Software Foundation. +% +% PbDlib is distributed in the hope that it will be useful, +% but WITHOUT ANY WARRANTY; without even the implied warranty of +% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +% GNU General Public License for more details. +% +% You should have received a copy of the GNU General Public License +% along with PbDlib. If not, see <http://www.gnu.org/licenses/>. + +addpath('./m_fcts/'); + + +%% Parameters +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +nbData = 200; %Number of datapoints +nbVarPos = 2; %Dimension of position data (here: x1,x2) +nbDeriv = 2; %Number of static and dynamic features (nbDeriv=2 for [x,dx] and u=ddx) +nbVar = nbVarPos * nbDeriv; %Dimension of state vector +dt = 0.01; %Time step duration + +Sx = diag([1E-1, 1E-4, 1E-1*dt, 1E-4*dt]); %Model accurate on y and inaccurate on x +Sy = diag([1E-4, 1E-1]); %Sensor accurate on x and inaccurate on y + +[Vx,Dx] = eig(Sx); %Eigencomponents +[Vy,Dy] = eig(Sy); %Eigencomponents + + +%% Dynamical System settings (discrete version) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Integration with higher order Taylor series expansion +A1d = zeros(nbDeriv); +for i=0:nbDeriv-1 + A1d = A1d + diag(ones(nbDeriv-i,1),i) * dt^i * 1/factorial(i); %Discrete 1D +end +B1d = zeros(nbDeriv,1); +for i=1:nbDeriv + B1d(nbDeriv-i+1) = dt^i * 1/factorial(i); %Discrete 1D +end +A = kron(A1d, eye(nbVarPos)); %Discrete nD +B = kron(B1d, eye(nbVarPos)); %Discrete nD +C = [eye(nbVarPos), zeros(nbVar-nbVarPos, nbVarPos)]; %Position sensor +% C = eye(nbVarPos); %Position+velocity sensor + + +%% Generate data +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +u = [ones(1, nbData-1); zeros(1,nbData-1)]; %Simulate constant acceleration along x +xd = B * u(:,1); +for t=1:nbData-1 + xd(:,t+1) = A * xd(:,t) + B * u(:,t); +end + + +%% Reproduction with Kalman filter (see e.g., https://www.mathworks.com/help/control/ug/kalman-filtering.html) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +ex = Vx * Dx.^.5 * randn(nbVar,nbData); %Simulate noise on state +ey = Vy * Dy.^.5 * randn(nbVarPos,nbData); %Simulate noise on sensor + +%Simulate system evolution without Kalman filter +x = zeros(nbVar,1); +for t=1:nbData-1 + %Time update + x = A * x + B * u(:,t) + Vx*Dx.^.5 * ex(:,t); %x[t+1|t] + %Simulate noise + y = C * x + Vy*Dy.^.5 * ey(:,t); %Sensor data + %Log data + r.y(:,t) = y; +end + +%Simulate system evolution with Kalman filter +x = zeros(nbVar,1); +P = Sx; +for t=1:nbData-1 + %Time update + x = A * x + B * u(:,t); %x[t+1|t] + P = A * P * A' + Sx; %P[t+1|t] + %Simulate noise + y = C * x + Vy*Dy.^.5 * ey(:,t); %Sensor data + %Measurement update + K = P * C' / (C * P * C' + Sy); %Kalman gain + x = x + K * (y - C * x); %x[t|t] + P = (eye(nbVar) - K * C) * P; %P[t|t] + %Log data + r.yf(:,t) = C * x; +end + + +%% Reproduction with Kalman filter as product of Gaussians (PoG) +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +x = zeros(nbVar,1); +S = Sx; +for t=1:nbData-1 + %Gaussian 1 + Mu(:,1) = A * x + B * u(:,t); + Sigma(:,:,1) = A * S * A' + Sx; + Q(:,:,1) = inv(A * S * A' + Sx); + QMu(:,1) = Q(:,:,1) * Mu(:,1); + + %Simulate noise + y = C * Mu(:,1) + Vy*Dy.^.5 * ey(:,t); %Sensor data + + %Gaussian 2 + Mu(:,2) = pinv(C) * y; + Sigma(:,:,2) = pinv(C) * Sy * pinv(C)'; + Q(:,:,2) = C' / Sy * C; + QMu(:,2) = C' / Sy * y; + + %Product of Gaussians (PoG) + QTmp = zeros(nbVar); + MuTmp = zeros(nbVar,1); + for i=1:2 + QTmp = QTmp + Q(:,:,i); + MuTmp = MuTmp + QMu(:,i); + end + S = inv(QTmp); + x = S * MuTmp; + + %Log data + r.yf2(:,t) = C * x; + r.Mu(:,:,t) = Mu; + r.Sigma(:,:,:,t) = Sigma; + r.S(:,:,t) = S; +end + + +%% Plot 2D +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +figure('position',[10 10 2200 1200],'color',[1 1 1]); +subplot(nbVarPos+1,1,1); hold on; axis off; +h(1) = plot(xd(1,:), xd(2,:), '-','linewidth',2,'color',[.7 .7 .7]); +h(2) = plot(r.y(1,:), r.y(2,:), '-','linewidth',1,'color',[.2 .2 .2]); +h(3) = plot(r.yf(1,:), r.yf(2,:), '-','linewidth',2,'color',[.8 0 0]); +h(4) = plot(r.yf2(1,:), r.yf2(2,:), '--','linewidth',2,'color',[0 .6 0]); +for t=round(linspace(1,nbData-1,5)) + hh(1,:) = plotGMM(r.Mu(1:2,1,t), r.Sigma(1:2,1:2,1,t), [.8 0 .8], .3); %Model + hh(2,:) = plotGMM(r.Mu(1:2,2,t), r.Sigma(1:2,1:2,2,t), [0 .8 .8], .3); %Sensor + hh(3,:) = plotGMM(r.yf2(:,t), r.S(1:2,1:2,t), [.8 0 0], .6); %Product +end +legend([h, hh(:,1)'], {'Ground truth','Observations','Filtered process','Filtered process (computed with PoG)','Model covariance','Sensor covariance','Resulting PoG'},'location','eastoutside','fontsize',14); +axis equal; + +%Timeline plot +labList = {'$x_1$','$x_2$','$\dot{x}_1$','$\dot{x}_2$','$\ddot{x}_1$','$\ddot{x}_2$'}; +for j=1:nbVarPos + subplot(nbVarPos+1,1,1+j); hold on; + plot(xd(j,:), '-','linewidth',.5,'color',[.7 .7 .7]); + plot(r.y(j,:), '-','linewidth',1,'color',[0 0 0]); + plot(r.yf(j,:), '-','linewidth',2,'color',[.8 0 0]); + plot(r.yf2(j,:), '--','linewidth',2,'color',[0 .6 0]); + axis([1, nbData, min(x(:))-5E-1, max(x(:))+5E-1]); + set(gca,'xtick',[],'ytick',[]); + xlabel('$t$','fontsize',16,'interpreter','latex'); + ylabel(labList{j},'fontsize',16,'interpreter','latex'); +end + +%print('-dpng','graphs/demo_Kalman01.png'); +pause; +close all; \ No newline at end of file