diff --git a/README.md b/README.md
index cd0c8fac7c03ff47a6847454e58fa50ab5a5a402..5641eef0b0a3cc03410df84a4dbdbcc4b8d8fcb9 100644
--- a/README.md
+++ b/README.md
@@ -277,10 +277,10 @@ If you find PbDlib useful for your research, please cite the related publication
 </p>
 
 <p><a name="ref-3">
-[3] Calinon, S. and Jaquier, N. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
+[3] Calinon, S. (2020). <strong>Gaussians on Riemannian Manifolds for Robot Learning and Adaptive Control</strong>. IEEE Robotics and Automation Magazine (RAM).
 </a><br>
-[[pdf]](http://calinon.ch/papers/Calinon-arXiv2019.pdf)
-[[bib]](http://calinon.ch/papers/Calinon-arXiv2019.bib)
+[[pdf]](http://calinon.ch/papers/Calinon-RAM2020.pdf)
+[[bib]](http://calinon.ch/papers/Calinon-RAM2020.bib)
 <br><strong>(Ref. for Riemannian manifolds)</strong>
 </p>
 
diff --git a/demos/demo_Riemannian_Gdp_vecTransp01.m b/demos/demo_Riemannian_Gdp_vecTransp01.m
index 6120e820ee8025973d540510d024dadda410347e..f0d80294e7b06a2c8cd9c127b5c8a7f8974e7f58 100644
--- a/demos/demo_Riemannian_Gdp_vecTransp01.m
+++ b/demos/demo_Riemannian_Gdp_vecTransp01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_Gdp_vecTransp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Hd_GMM01.m b/demos/demo_Riemannian_Hd_GMM01.m
index 9775aff6151ea8d88eee0327517a286baba7b339..1c787d90f08a937d8434714f7163526c18952d5f 100644
--- a/demos/demo_Riemannian_Hd_GMM01.m
+++ b/demos/demo_Riemannian_Hd_GMM01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_Hd_GMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Hd_interp01.m b/demos/demo_Riemannian_Hd_interp01.m
index c42a16dfe7f3438555802d91e3696bd5f7bc90ba..3f72c2d300d1967d398209bf8d0db6dcd7f3d8f8 100644
--- a/demos/demo_Riemannian_Hd_interp01.m
+++ b/demos/demo_Riemannian_Hd_interp01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_Hd_interp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S1_interp01.m b/demos/demo_Riemannian_S1_interp01.m
index fe8d1cbae070edac140bff7dd4007b78bc133d09..fbd0cb859c7271bd732b2ac9ccfe1e4bd37b5379 100644
--- a/demos/demo_Riemannian_S1_interp01.m
+++ b/demos/demo_Riemannian_S1_interp01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S1_interp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S1_interp02.m b/demos/demo_Riemannian_S1_interp02.m
index 81cdbfc8b4cb6b1ca0cfb6a05eda8912692b4cf8..a3e5119b63f27ff692c124ce0d7976fab9f43dda 100644
--- a/demos/demo_Riemannian_S1_interp02.m
+++ b/demos/demo_Riemannian_S1_interp02.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S1_interp02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_GMM01.m b/demos/demo_Riemannian_S2_GMM01.m
index 2943cd291ac60ed928ceec21e37d8fce781d84cd..f03349eebbf676ea5e75b3ac833809fad8fd7236 100644
--- a/demos/demo_Riemannian_S2_GMM01.m
+++ b/demos/demo_Riemannian_S2_GMM01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_GMR01.m b/demos/demo_Riemannian_S2_GMR01.m
index a9e0aba14bd697a2081c5bb318a34b670e71c551..4d50732d32618a25cad8cf580a14c72b0ad01025 100644
--- a/demos/demo_Riemannian_S2_GMR01.m
+++ b/demos/demo_Riemannian_S2_GMR01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_GMR02.m b/demos/demo_Riemannian_S2_GMR02.m
index 44d70eb2e4c25d2143418b83082a5fb5c2a0f32a..e6ff655d658653e7f2059192932476a70a3955bf 100644
--- a/demos/demo_Riemannian_S2_GMR02.m
+++ b/demos/demo_Riemannian_S2_GMR02.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_GMR03.m b/demos/demo_Riemannian_S2_GMR03.m
index f676538ff6f48551f34759cea56113114042608e..f75e1919b0a2ebd7441d1248248d3eab100cec35 100644
--- a/demos/demo_Riemannian_S2_GMR03.m
+++ b/demos/demo_Riemannian_S2_GMR03.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR03
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_GMR04.m b/demos/demo_Riemannian_S2_GMR04.m
index b40e14fb524aa748d2ad43974ecdd0e0f3c69631..304b07d897ced8f7d5a680e0129fd96deafbe04a 100644
--- a/demos/demo_Riemannian_S2_GMR04.m
+++ b/demos/demo_Riemannian_S2_GMR04.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GMR04
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_GaussProd01.m b/demos/demo_Riemannian_S2_GaussProd01.m
index 33bfa7406c91d4d7f971c731d9d7449bb9840019..7ac4ee6aef1bec1d438b8aaa76d46f66963b88c3 100644
--- a/demos/demo_Riemannian_S2_GaussProd01.m
+++ b/demos/demo_Riemannian_S2_GaussProd01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_GaussProd01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_TPGMM01.m b/demos/demo_Riemannian_S2_TPGMM01.m
index 98d505708391307e1495e9a921667b2ed1c03312..0a508246407f1978d0726a5ebe664e2ec2e1158a 100644
--- a/demos/demo_Riemannian_S2_TPGMM01.m
+++ b/demos/demo_Riemannian_S2_TPGMM01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_TPGMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_TPGMM02.m b/demos/demo_Riemannian_S2_TPGMM02.m
index 58e707e54be57becead48f08e73c8281a6213d40..ef41fa26a748222d77512e16e1450bbb4baa973a 100644
--- a/demos/demo_Riemannian_S2_TPGMM02.m
+++ b/demos/demo_Riemannian_S2_TPGMM02.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_TPGMM02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_batchLQR01.m b/demos/demo_Riemannian_S2_batchLQR01.m
index 1fc96ecd430a93468e7ab7a282e322cce460144c..9abce44ed67a95e78af137e55ecc80ab85c4806b 100644
--- a/demos/demo_Riemannian_S2_batchLQR01.m
+++ b/demos/demo_Riemannian_S2_batchLQR01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_batchLQR02.m b/demos/demo_Riemannian_S2_batchLQR02.m
index 5601c2067ba0f4fc8413f37f87409aaf0fa0bf03..380c7871184637c8042e021acd6fc76d615e8510 100644
--- a/demos/demo_Riemannian_S2_batchLQR02.m
+++ b/demos/demo_Riemannian_S2_batchLQR02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_batchLQR03.m b/demos/demo_Riemannian_S2_batchLQR03.m
index cd1ec900bcda741980d67b9a8993328686379b88..db0578ca83e8f302373fc9f6244aecfd23da45ea 100644
--- a/demos/demo_Riemannian_S2_batchLQR03.m
+++ b/demos/demo_Riemannian_S2_batchLQR03.m
@@ -4,7 +4,7 @@ function demo_Riemannian_S2_batchLQR03
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
index bfe5cb1e3ab9d756ec0812917ba1be2e5cefe827..ef82dd79c4229494e29002023635e0526d92ee3f 100644
--- a/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
+++ b/demos/demo_Riemannian_S2_batchLQR_Bezier01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_batchLQR_Bezier01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_infHorLQR01.m b/demos/demo_Riemannian_S2_infHorLQR01.m
index 0e3b1f816efd8ae9ce617b7672a7254e6b8217d0..361f79ce5b1bcf0d8eac42b866a62e3313a3f5c6 100644
--- a/demos/demo_Riemannian_S2_infHorLQR01.m
+++ b/demos/demo_Riemannian_S2_infHorLQR01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_infHorLQR01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S2_vecTransp01.m b/demos/demo_Riemannian_S2_vecTransp01.m
index 8fbc5b1818dff22a517e7f698d69e6103eee445f..54dc29e844914cd3979885055a58bbab41749006 100644
--- a/demos/demo_Riemannian_S2_vecTransp01.m
+++ b/demos/demo_Riemannian_S2_vecTransp01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S2_vecTransp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S3_GMM01.m b/demos/demo_Riemannian_S3_GMM01.m
index 3f4f063f138799dca08a438fd748749d1c70bd6e..ecf960cc3ef7b0e3301a194277b1062e1db0643a 100644
--- a/demos/demo_Riemannian_S3_GMM01.m
+++ b/demos/demo_Riemannian_S3_GMM01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S3_GMR01.m b/demos/demo_Riemannian_S3_GMR01.m
index e6bebee7311423f4a9f8c23b316d48830e4e39fe..c0662cf61facad74d7f83739ac8f2e6835bb3c99 100644
--- a/demos/demo_Riemannian_S3_GMR01.m
+++ b/demos/demo_Riemannian_S3_GMR01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMR01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S3_GMR02.m b/demos/demo_Riemannian_S3_GMR02.m
index 6f9943ee1ceddb9fae8dd6c3fe596dcaf2327754..6566f25c00e400c74aa635e25e8df9ed1708f22c 100644
--- a/demos/demo_Riemannian_S3_GMR02.m
+++ b/demos/demo_Riemannian_S3_GMR02.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S3_GMR02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S3_infHorLQR01.m b/demos/demo_Riemannian_S3_infHorLQR01.m
index af84917957604970599de35af532cea123c4d295..66e12788a7886903f3ae64497bd8721377bba3a4 100644
--- a/demos/demo_Riemannian_S3_infHorLQR01.m
+++ b/demos/demo_Riemannian_S3_infHorLQR01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S3_infHorLQR01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S3_interp01.m b/demos/demo_Riemannian_S3_interp01.m
index 5a8117c8bf993c5ea2ab2e5f9a6291478b243748..979fc7e4dbfb2be592a3de1f86b1bb6781fc0bd3 100644
--- a/demos/demo_Riemannian_S3_interp01.m
+++ b/demos/demo_Riemannian_S3_interp01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S3_interp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_S3_vecTransp01.m b/demos/demo_Riemannian_S3_vecTransp01.m
index 68a1e656b058de18f7115148d9ab637f630a9709..84e9e97bf2206d82de23d9807b3f3fdbdcf7030a 100644
--- a/demos/demo_Riemannian_S3_vecTransp01.m
+++ b/demos/demo_Riemannian_S3_vecTransp01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_S3_vecTransp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_SE2_GMM01.m b/demos/demo_Riemannian_SE2_GMM01.m
index 35c864c9a65a5c203bfbaa46ed83426eb0ce9bd4..48a0e3f818cab54a80a9d697998b8603f733bfe5 100644
--- a/demos/demo_Riemannian_SE2_GMM01.m
+++ b/demos/demo_Riemannian_SE2_GMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_SE2_GMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_SE2_interp01.m b/demos/demo_Riemannian_SE2_interp01.m
index 3ec31e162574598e085a49844c9fb367a7326c63..bb95e0e872c1afa753c47475e55a1a279978616e 100644
--- a/demos/demo_Riemannian_SE2_interp01.m
+++ b/demos/demo_Riemannian_SE2_interp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_SE2_interp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_SOd_interp01.m b/demos/demo_Riemannian_SOd_interp01.m
index 9e080720b2d7f0dc83584fea53bbc0c814f0c43a..2f91b983d8e1c8c43bd554a671344fdccfd09737 100644
--- a/demos/demo_Riemannian_SOd_interp01.m
+++ b/demos/demo_Riemannian_SOd_interp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_SOd_interp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_GMM01.m b/demos/demo_Riemannian_Sd_GMM01.m
index 8345e9904785fc5806419a87ac0bd9f733594f43..85a41a36297945f297ac0f94d4cabfa109940f03 100644
--- a/demos/demo_Riemannian_Sd_GMM01.m
+++ b/demos/demo_Riemannian_Sd_GMM01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_GMM02.m b/demos/demo_Riemannian_Sd_GMM02.m
index 454e07d3e656fc954c72379bb716defaeba8890c..7241b0d15b2294e863b5f36df4b17bd04fd2353d 100644
--- a/demos/demo_Riemannian_Sd_GMM02.m
+++ b/demos/demo_Riemannian_Sd_GMM02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMM02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_GMR01.m b/demos/demo_Riemannian_Sd_GMR01.m
index e01b7577824e9d74cdd8a890547dd1c15e5716af..3018bd4cf79edaee9d7ac3c0dad4b18d4bdbc67d 100644
--- a/demos/demo_Riemannian_Sd_GMR01.m
+++ b/demos/demo_Riemannian_Sd_GMR01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMR01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_GMR02.m b/demos/demo_Riemannian_Sd_GMR02.m
index c3fd241b70d622aed8e5d7aac87c4efa4de33cb8..7b3ab371854fa42be8221fd6899c644e82dda6a4 100644
--- a/demos/demo_Riemannian_Sd_GMR02.m
+++ b/demos/demo_Riemannian_Sd_GMR02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_GMR02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_GaussProd01.m b/demos/demo_Riemannian_Sd_GaussProd01.m
index 066ca3d1d6fbfe1225aff73733b2ca0aeff9c334..93335972eed36bbb998dce5574b991d1317e68e5 100644
--- a/demos/demo_Riemannian_Sd_GaussProd01.m
+++ b/demos/demo_Riemannian_Sd_GaussProd01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_Sd_GaussProd01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_MPC01.m b/demos/demo_Riemannian_Sd_MPC01.m
index 9c3957cff8de68f9ec9751eaa43413b39a4fedf2..a004551e14ebd410386569f8282896c4d0bf742d 100644
--- a/demos/demo_Riemannian_Sd_MPC01.m
+++ b/demos/demo_Riemannian_Sd_MPC01.m
@@ -6,7 +6,7 @@ function demo_Riemannian_Sd_MPC01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_MPC_infHor01.m b/demos/demo_Riemannian_Sd_MPC_infHor01.m
index 60eb3a73459315af79dfcf32c8b81a6315afc2a5..95fe04ed134aeac2a46aef8567be03d03e358533 100644
--- a/demos/demo_Riemannian_Sd_MPC_infHor01.m
+++ b/demos/demo_Riemannian_Sd_MPC_infHor01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_MPC_infHor01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_interp01.m b/demos/demo_Riemannian_Sd_interp01.m
index 68586869c73daf67d8ac4b06cb05df6dda3a1d0f..76f843282a2ea9f39c79c99c63e410567265f2cc 100644
--- a/demos/demo_Riemannian_Sd_interp01.m
+++ b/demos/demo_Riemannian_Sd_interp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_interp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_interp02.m b/demos/demo_Riemannian_Sd_interp02.m
index 009cc81ea52e09b52fd6dff9672edde644cea813..662d1d833a1c88b1aaf4160cdba85a7954d03349 100644
--- a/demos/demo_Riemannian_Sd_interp02.m
+++ b/demos/demo_Riemannian_Sd_interp02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_interp02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_vecTransp01.m b/demos/demo_Riemannian_Sd_vecTransp01.m
index b644b7863156337953d15fbeeef94c368cc24620..eddd03cceff9a694b146a83f1e1cb3fa18c40964 100644
--- a/demos/demo_Riemannian_Sd_vecTransp01.m
+++ b/demos/demo_Riemannian_Sd_vecTransp01.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_vecTransp01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_Sd_vecTransp02.m b/demos/demo_Riemannian_Sd_vecTransp02.m
index f1474483f94f74ab28b110627e2f27d9e3d2bcca..751f6cf485c078de5172d0d3ea762748ff3d4f34 100644
--- a/demos/demo_Riemannian_Sd_vecTransp02.m
+++ b/demos/demo_Riemannian_Sd_vecTransp02.m
@@ -4,7 +4,7 @@ function demo_Riemannian_Sd_vecTransp02
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_Riemannian_pose_GMM01.m b/demos/demo_Riemannian_pose_GMM01.m
index d72dbfd4520b956713d369c8de1a361268793d91..64866cfeff948bf9069cef4b0e5ba10a216d900c 100644
--- a/demos/demo_Riemannian_pose_GMM01.m
+++ b/demos/demo_Riemannian_pose_GMM01.m
@@ -3,7 +3,7 @@ function demo_Riemannian_pose_GMM01
 %
 % If this code is useful for your research, please cite the related publication:
 % @article{Calinon20RAM,
-% 	author="Calinon, S. and Jaquier, N.",
+% 	author="Calinon, S.",
 % 	title="Gaussians on {R}iemannian Manifolds for Robot Learning and Adaptive Control",
 % 	journal="{IEEE} Robotics and Automation Magazine ({RAM})",
 % 	year="2020",
diff --git a/demos/demo_ergodicControl_1D01.m b/demos/demo_ergodicControl_1D01.m
index a25a8d9c0f65d179ea9c3e2cfcf040ecf699eaec..3a5902fd5025e43589fa1ee58b4760f6d56c60b1 100644
--- a/demos/demo_ergodicControl_1D01.m
+++ b/demos/demo_ergodicControl_1D01.m
@@ -36,7 +36,7 @@ addpath('./m_fcts/');
 
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 4000; %Number of datapoints
+nbData = 8000; %Number of datapoints
 nbFct = 50; %Number of basis functions along x and y
 nbStates = 2; %Number of Gaussians to represent the spatial distribution
 dt = 1E-2; %Time step
@@ -44,7 +44,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
 om = 2 .* pi ./ L; %omega
 u_max = 1E0; %Maximum speed allowed 
-x = .1; %Initial position
 
 %Desired spatial distribution represented as a mixture of Gaussians
 Mu(:,1) = 0.7 *1;
@@ -59,69 +58,161 @@ Priors = ones(1,nbStates) ./ nbStates; %Mixing coefficients
 rg = [0:nbFct-1]';
 Lambda = (rg.^2 + 1).^-1; %Weighting vector (Eq.(15)
 
-HK = L; %Rescaling term (as scalar)
-% HK = [1; sqrt(.5)*ones(nbFct-1,1)]; %Rescaling term (as normalizing vector)
-
 %Explicit description of w_hat by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 kk = rg .* om;
 w_hat = zeros(nbFct,1);
 for j=1:nbStates
 	w_hat = w_hat + Priors(j) .* cos(kk .* Mu(:,j)) .* exp(-.5 .* kk.^2 .* Sigma(:,j)); %Eq.(22)
 end
-w_hat = w_hat ./ HK;
+w_hat = w_hat ./ L;
+
+% %Alternative computation of w_hat by discretization (for verification)
+% nbRes = 1000;
+% x = linspace(xlim(1), xlim(2), nbRes); %Spatial range
+% g = zeros(1,nbRes);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution 
+% end
+% phi_inv = cos(rg * x .* om) ./ L ./ nbRes;
+% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
+% 
+% % w_hat = zeros(nbFct,1);
+% % for n=1:nbFct
+% % 	w_hat(n) = sum(g .* cos(x .* rg(n) .* om)); 
+% % end
+% % w_hat = w_hat ./ L ./ nbRes; %Fourier coefficients of spatial distribution
+
+%Fourier basis functions (for a discretized map)
+nbRes = 100;
+xm = linspace(xlim(1), xlim(2), nbRes); %Spatial range
+phim = cos(rg * xm .* om) .* 2; %Fourier basis functions
+phim(2:end,:) = phim(2:end,:) .* 2;
+
+%Desired spatial distribution 
+g = w_hat' * phim;
+
+% %Alternative computation of g
+% g = zeros(1,nbRes);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(xm', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
+% 	%(2.*pi.*s1).^-.5 .*  exp(-.5 .* s1.^-1 .* (X - m1).^2);
+% end
 
 
 %% Ergodic control 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x = .1; %Initial position
 wt = zeros(nbFct, 1);
 for t=1:nbData
 	r.x(:,t) = x; %Log data
 	
 	%Fourier basis functions and derivatives for each dimension (only cosine part on [0,L/2] is computed since the signal is even and real by construction) 
 	phi = cos(x * rg .* om); %Eq.(18)
-	dphi = -sin(x * rg .* om) .* rg .* om;
-	
-% 	%Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) 
-% 	fx = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% 	gradf = real(-exp(-i .* x * rg .* om) .* i .* rg .* om);
+	dphi = -sin(x * rg .* om) .* rg .* om; %Gradient of basis functions
 
-	wt = wt + phi ./ HK;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
+	wt = wt + phi ./ L;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
 
 % 	%Controller with ridge regression formulation
-% 	u = -gradf' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
+% 	u = -dphi' * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
 	
 	%Controller with constrained velocity norm
 	u = -dphi' * (Lambda .* (wt./t - w_hat)); %Eq.(24)
 	u = u .* u_max ./ (norm(u)+1E-2); %Velocity command
 	
 	x = x + u .* dt; %Update of position
+	r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution 
+% 	r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
 end
-% % r.G = real(A * ck' / nbData); 
-% invA = exp(-Xm' * KX *i*2*pi) / nbData ./ repmat(HK,nbRes,1);
-% r.G = real(invA * ck'); 
+
+% %Alternative computation of desired and reconstructed spatial distribution 
+% g = zeros(1,nbRes);
+% for n=1:nbFct
+% 	g = g + w_hat(n) .*  cos(xm .* rg(n) .* om);
+% end
+% r.g = zeros(1,nbRes);
+% for n=1:nbFct
+% 	r.g = r.g + (wt(n)./nbData) .*  cos(xm .* rg(n) .* om);
+% end
 
 
-%% Plot
+%% Plot (static)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1800,900],'color',[1 1 1]); 
-%Plot signal
-subplot(1,3,[1,2]); hold on; 
-plot(1:nbData, r.x, '-','linewidth',3,'color',[.2 .2 .2]);
-axis([1, nbData, xlim']);
-xlabel('t','fontsize',18); ylabel('x','fontsize',18); 
-set(gca,'xtick',[],'ytick',[]);
+s = nbData; %Reconstruction at time step s
+T = nbData; %Size of time window
+figure('position',[10,10,1200,1200],'color',[1 1 1]); 
 %Plot distribution
-subplot(1,3,3); hold on; 
-nbRes = 100;
-x = linspace(xlim(1),xlim(2),nbRes);
-g = zeros(1,nbRes);
-for k=1:nbStates
-	g = g + Priors(k) .* mvnpdf(x', Mu(:,k)', Sigma(:,k))'; %Spatial distribution
-end
-plot(g, x, '-','linewidth',3,'color',[.8 0 0]);
-xlabel('\phi(x)','fontsize',18); ylabel('x','fontsize',18); 
-set(gca,'xtick',[],'ytick',[]);
-
+subplot(3,1,1); hold on; 
+h(1) = plot(xm, g, '-','linewidth',3,'color',[.8 0 0]);
+h(2) = plot(xm, r.g(:,s), '-','linewidth',3,'color',[0 0 0]);
+legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest');
+axis([xlim', -.3, max(g)*1.2]);
+xlabel('$x$','interpreter','latex','fontsize',28); 
+ylabel('$g(x)$','interpreter','latex','fontsize',28);  
+set(gca,'linewidth',2,'xtick',[],'ytick',[]);
+%Plot signal
+subplot(3,1,[2,3]); hold on; box on;
+plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]);
+plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0]);
+axis([xlim', 1, T]);
+xlabel('$x$','interpreter','latex','fontsize',28);   
+set(gca,'linewidth',2,'xtick',[],'ytick',[1,T],'yticklabel',{'t-T','t'},'fontsize',24);
 % print('-dpng','graphs/ergodicControl_1D01.png'); 
+
+% %Plot error
+% figure; hold on;
+% plot(r.e(200:end));
+
+% %Plot decomposition of desired distribution
+% dg = 4;
+% figure('position',[10,10,900,1200],'color',[1 1 1]); hold on; axis off;
+% %Plot distribution
+% plot(xm, g*2, '-','linewidth',3,'color',[.8 0 0]);
+% for n=1:6
+% 	plot(xm, .3*phim(n,:)-dg*n,'-','linewidth',2,'color',[0 .6 0]);
+% end
+% % print('-dpng','graphs/ergodicControl_1DbasisFcts01.png'); 
+
+
+% %% Plot (animated)
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% T = 2000; %Size of time window
+% s0 = 1; %Animation from time step s0
+% figure('position',[10,10,1200,1200],'color',[1 1 1]); 
+% %Plot distribution
+% subplot(3,1,1); hold on; 
+% h(1) = plot(xm, g, '-','linewidth',3,'color',[.8 0 0]);
+% h(2) = plot(xm, r.g(:,s0), '-','linewidth',3,'color',[0 0 0]);
+% axis([xlim', -.3, max(g)*1.2]);
+% xlabel('$x$','interpreter','latex','fontsize',28); 
+% ylabel('$g(x)$','interpreter','latex','fontsize',28);  
+% set(gca,'linewidth',2,'xtick',[],'ytick',[]);
+% %Plot signal
+% subplot(3,1,[2,3]); hold on; box on;
+% axis([xlim', 1, T]);
+% xlabel('$x$','interpreter','latex','fontsize',28);   
+% set(gca,'linewidth',2,'xtick',[],'ytick',[1,T],'yticklabel',{'t-T','t'},'fontsize',24);
+% %Animation
+% hs=[]; id=1;
+% for s = s0:500:nbData 
+% 	%Plot distribution
+% 	subplot(3,1,1); hold on; 
+% 	delete(h(2));
+% 	h(2) = plot(xm, r.g(:,s), '-','linewidth',3,'color',[0 0 0]);
+% 	legend(h,{'Desired','Reconstructed'},'fontsize',18,'location','northwest');
+% 	%Plot signal
+% 	subplot(3,1,[2,3]); hold on; box on;
+% 	delete(hs);
+% 	if s>T
+% 		hs(1) = plot(r.x(:,s-T+1:s), 1:T, '-','linewidth',3,'color',[0 0 0]);
+% 	else
+% 		hs(1) = plot(r.x(:,1:s), T-s+1:T, '-','linewidth',3,'color',[0 0 0]);
+% 	end
+% 	hs(2) = plot(r.x(:,s), T, '.','markersize',28,'color',[0 0 0]);
+% 	drawnow;
+% % 	print('-dpng',['graphs/anim/ergodicControl_1Danim' num2str(id,'%.3d') '.png']);
+% %		id = id + 1;
+% % 	pause(.1); 
+% end
+
 pause;
 close all;
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_2D01.m b/demos/demo_ergodicControl_2D01.m
index f662e1496086b971e874a34581f5a28303025a90..0a6858c84f72c7e13902a52197043b0b670d810b 100644
--- a/demos/demo_ergodicControl_2D01.m
+++ b/demos/demo_ergodicControl_2D01.m
@@ -36,7 +36,7 @@ addpath('./m_fcts/');
 
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-nbData = 4000; %Number of datapoints
+nbData = 8000; %Number of datapoints
 nbFct = 20; %Number of basis functions along x and y
 nbVar = 2; %Dimension of datapoint
 nbStates = 2; %Number of Gaussians to represent the spatial distribution
@@ -45,8 +45,7 @@ dt = 1E-2; %Time step
 xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dimension in this implementation)
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
 om = 2 .* pi ./ L; %omega
-u_max = 5E0; %Maximum speed allowed 
-x = [.1; .3]; %Initial position
+u_max = 5E1; %Maximum speed allowed 
 
 %Desired spatial distribution represented as a mixture of Gaussians
 Mu(:,1) = [.3; .7]; 
@@ -63,10 +62,6 @@ rg = 0:nbFct-1;
 [KX(1,:,:), KX(2,:,:)] = ndgrid(rg, rg);
 Lambda = (KX(1,:).^2 + KX(2,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15))
 
-HK = L^nbVar; %Rescaling term (as scalar)
-% hk = [1; sqrt(.5)*ones(nbFct-1,1)];
-% HK = hk(xx,1) .* hk(yy,1); %Rescaling term (as normalizing matrix)
-
 %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1])
 op = hadamard(2^(nbVar-1));
@@ -81,11 +76,46 @@ for j=1:nbStates
 		w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
 	end
 end
-w_hat = w_hat ./ HK ./ size(op,2);
+w_hat = w_hat ./ L^nbVar ./ size(op,2);
+
+% %Alternative computation of w_hat by discretization (for verification)
+% nbRes = 100;
+% xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
+% [xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
+% phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* 2^nbVar; %Fourier basis functions
+% g = zeros(1,nbRes^nbVar);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
+% end
+% phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) ./ L^nbVar ./ nbRes^nbVar;
+% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
+
+%Fourier basis functions (for a discretized map)
+nbRes = 40;
+xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
+[xm(1,:,:), xm(2,:,:)] = ndgrid(xm1d, xm1d); %Spatial range
+phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* 2^nbVar; %Fourier basis functions
+% % phim(2:end,:) = phim(2:end,:) .* 2;
+% phim = phim .* 2^nbVar;
+% phim(1:nbFct,:) = phim(1:nbFct,:) .* .5;
+% phim(1:nbFct:end,:) = phim(1:nbFct:end,:) .* .5;
+hk = [1; 2*ones(nbFct-1,1)];
+HK = hk(xx(:)) .* hk(yy(:)); 
+phim = phim .* repmat(HK,[1,nbRes^nbVar]);
+
+%Desired spatial distribution 
+g = w_hat' * phim;
+
+% %Alternative computation of g
+% g = zeros(1,nbRes^nbVar);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
+% end
 
 
 %% Ergodic control 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x = [.1; .3]; %Initial position
 wt = zeros(nbFct^nbVar, 1);
 for t=1:nbData
 	r.x(:,t) = x; %Log data
@@ -94,12 +124,8 @@ for t=1:nbData
 	phi1 = cos(x * rg .* om); %Eq.(18)
 	dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
 	
-% 	%Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) 
-% 	phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% 	dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
-
-	dphi = [dphi1(1,xx) .* phi1(2,yy); phi1(1,xx) .* dphi1(2,yy)]; %gradient of basis functions
-	wt = wt + (phi1(1,xx) .* phi1(2,yy))' ./ HK;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
+	dphi = [dphi1(1,xx) .* phi1(2,yy); phi1(1,xx) .* dphi1(2,yy)]; %Gradient of basis functions
+	wt = wt + (phi1(1,xx) .* phi1(2,yy))' ./ L.^nbVar;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
 
 % 	%Controller with ridge regression formulation
 % 	u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 5E-1; %Velocity command
@@ -109,33 +135,69 @@ for t=1:nbData
 	u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
 	
 	x = x + u .* dt; %Update of position
+	r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution 
+% 	r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
 end
 
 
-%% Plot
+%% Plot (static)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure('position',[10,10,1200,1200]); hold on; axis off;
-plotGMM(Mu, Sigma, [.2 .2 .2], .3);
+colormap(repmat(linspace(1,.4,64),3,1)');
+% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+% plotGMM(Mu, Sigma, [.2 .2 .2], .3);
 plot(r.x(1,:), r.x(2,:), '-','linewidth',1,'color',[0 0 0]);
 plot(r.x(1,1), r.x(2,1), '.','markersize',15,'color',[0 0 0]);
 axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal;
 % print('-dpng','graphs/ergodicControl_2D01.png'); 
 
-%Plot Fourier series coefficients
-figure('position',[1220,10,1300,700]);
-colormap(repmat(linspace(1,.4,64),3,1)');
-subplot(1,2,1); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20);
-imagesc(reshape(w_hat,nbFct,nbFct));
-axis tight; axis equal; axis ij;
-subplot(1,2,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20);
-imagesc(reshape(wt./t,[nbFct,nbFct]));
-axis tight; axis equal; axis ij;
-
-% %Plot Fourier series coefficients (alternative version)
-% figure('position',[1220,10,1300,700]); hold on; axis off;
+% %Plot g as image
+% figure; hold on;
+% imagesc(reshape(g,[nbRes,nbRes])');
+% axis tight; axis equal; %axis ij;
+
+% %Plot g as graph
+% figure('position',[10,10,2300,1300]); 
+% subplot(1,2,1); hold on; rotate3d on;
+% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(g,[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+% view(3); axis vis3d;
+% subplot(1,2,2); hold on; rotate3d on;
+% surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), reshape(r.g(:,end),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+% view(3); axis vis3d;
+
+% %Plot Fourier series coefficients
+% figure('position',[1220,10,1300,700]);
 % colormap(repmat(linspace(1,.4,64),3,1)');
-% imagesc([w_hat, reshape(wt./t,[nbFct,nbFct])]); 
+% subplot(1,2,1); hold on; axis off; title('$\hat{w}$','interpreter','latex','fontsize',20);
+% imagesc(reshape(w_hat,nbFct,nbFct));
 % axis tight; axis equal; axis ij;
+% subplot(1,2,2); hold on; axis off; title('$w$','interpreter','latex','fontsize',20);
+% imagesc(reshape(wt./t,[nbFct,nbFct]));
+% axis tight; axis equal; axis ij;
+% 
+% % %Plot Fourier series coefficients (alternative version)
+% % figure('position',[1220,10,1300,700]); hold on; axis off;
+% % colormap(repmat(linspace(1,.4,64),3,1)');
+% % imagesc([w_hat, reshape(wt./t,[nbFct,nbFct])]); 
+% % axis tight; axis equal; axis ij;
+
+
+% %% Plot (animated)
+% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% figure('position',[10,10,1200,1200]); hold on; axis off;
+% colormap(repmat(linspace(1,.4,64),3,1)');
+% plot(r.x(1,1), r.x(2,1), '.','markersize',15,'color',[0 0 0]);
+% axis([xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal;
+% h=[]; id=1;
+% for t=1:10:nbData
+% 	delete(h);
+% 	h(1) = surface(squeeze(xm(1,:,:)), squeeze(xm(2,:,:)), zeros([nbRes,nbRes]), reshape(r.g(:,t),[nbRes,nbRes]), 'FaceColor','interp','EdgeColor','interp');
+% 	h(2) = plot(r.x(1,1:t), r.x(2,1:t), '-','linewidth',1,'color',[0 0 0]);
+% 	drawnow;
+% % 	print('-dpng',['graphs/anim/ergodicControl_2Danim' num2str(id,'%.3d') '.png']);
+% %		id = id + 1;
+% end
 
 pause;
 close all;
\ No newline at end of file
diff --git a/demos/demo_ergodicControl_3D01.m b/demos/demo_ergodicControl_3D01.m
index 8865f5bbec8a5bbbc918c3662584007e90c2633c..199e65319c5e01db6855c5eea50a466b4daeda79 100644
--- a/demos/demo_ergodicControl_3D01.m
+++ b/demos/demo_ergodicControl_3D01.m
@@ -37,7 +37,7 @@ addpath('./m_fcts/');
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 nbData = 4000; %Number of datapoints
-nbFct = 10; %Number of basis functions along x and y
+nbFct = 20; %Number of basis functions along x and y
 nbVar = 3; %Dimension of datapoint
 nbStates = 2; %Number of Gaussians to represent the spatial distribution
 sp = (nbVar + 1) / 2; %Sobolev norm parameter
@@ -46,7 +46,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
 om = 2 .* pi ./ L; %omega
 u_max = 1E1; %Maximum speed allowed 
-x = [.7; .1; .5]; %Initial position
 
 %Desired spatial distribution represented as a mixture of Gaussians
 Mu(:,1) = [.4; .5; .7]; 
@@ -63,7 +62,7 @@ rg = 0:nbFct-1;
 [KX(1,:,:,:), KX(2,:,:,:), KX(3,:,:,:)] = ndgrid(rg, rg, rg);
 Lambda = (KX(1,:).^2 + KX(2,:).^2 + KX(3,:).^2 + 1)'.^-sp; %Weighting vector (Eq.(15))
 
-HK = L^nbVar; %Rescaling term (as scalar)
+% HK = L^nbVar; %Rescaling term (as scalar)
 % hk = [1; sqrt(.5)*ones(nbFct-1,1)];
 % HK = hk(xx,1) .* hk(yy,1) .* hk(zz,1); %Rescaling term (as normalizing matrix)
 
@@ -81,11 +80,42 @@ for j=1:nbStates
 		w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
 	end
 end
-w_hat = w_hat ./ HK ./ size(op,2);
+w_hat = w_hat ./ L^nbVar ./ size(op,2);
+
+% %Alternative computation of w_hat by discretization (for verification)
+% nbRes = 20;
+% xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
+% [xm(1,:,:,:), xm(2,:,:,:), xm(3,:,:,:)] = ndgrid(xm1d, xm1d, xm1d); %Spatial range
+% phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) .* 2^nbVar; %Fourier basis functions
+% g = zeros(1,nbRes^nbVar);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
+% end
+% phi_inv = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) ./ L^nbVar ./ nbRes^nbVar;
+% w_hat = phi_inv * g'; %Fourier coefficients of spatial distribution
+
+%Fourier basis functions (for a discretized map)
+nbRes = 20;
+xm1d = linspace(xlim(1), xlim(2), nbRes); %Spatial range for 1D
+[xm(1,:,:,:), xm(2,:,:,:), xm(3,:,:,:)] = ndgrid(xm1d, xm1d, xm1d); %Spatial range
+phim = cos(KX(1,:)' * xm(1,:) .* om) .* cos(KX(2,:)' * xm(2,:) .* om) .* cos(KX(3,:)' * xm(3,:) .* om) .* 2^nbVar; %Fourier basis functions
+hk = [1; 2*ones(nbFct-1,1)];
+HK = hk(xx(:)) .* hk(yy(:)) .* hk(zz(:)); 
+phim = phim .* repmat(HK,[1,nbRes^nbVar]);
+
+%Desired spatial distribution 
+g = w_hat' * phim;
+
+% %Alternative computation of g
+% g = zeros(1,nbRes^nbVar);
+% for k=1:nbStates
+% 	g = g + Priors(k) .* mvnpdf(xm(:,:)', Mu(:,k)', Sigma(:,:,k))'; %Spatial distribution
+% end
 
 
 %% Ergodic control 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x = [.7; .1; .5]; %Initial position
 wt = zeros(nbFct^nbVar, 1);
 for t=1:nbData
 	r.x(:,t) = x; %Log data
@@ -94,12 +124,8 @@ for t=1:nbData
 	phi1 = cos(x * rg .* om); %Eq.(18)
 	dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
 	
-% 	%Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) 
-% 	phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% 	dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
-
-	dphi = [dphi1(1,xx) .* phi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* dphi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* phi1(2,yy) .* dphi1(3,zz)]; %gradient of basis functions
-	wt = wt + (phi1(1,xx) .* phi1(2,yy) .* phi1(3,zz))' ./ HK;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
+	dphi = [dphi1(1,xx) .* phi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* dphi1(2,yy) .* phi1(3,zz); phi1(1,xx) .* phi1(2,yy) .* dphi1(3,zz)]; %Gradient of basis functions
+	wt = wt + (phi1(1,xx) .* phi1(2,yy) .* phi1(3,zz))' ./ L^nbVar;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
 	
 % 	%Controller with ridge regression formulation
 % 	u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command
@@ -109,18 +135,38 @@ for t=1:nbData
 	u = u .* u_max ./ (norm(u)+1E-1); %Velocity command
 	
 	x = x + u .* dt; %Update of position
+
+% 	r.g(:,t) = (wt./t)' * phim; %Reconstructed spatial distribution 
+% 	r.e(t) = sum(sum((wt./t - w_hat).^2 .* Lambda)); %Reconstruction error evaluation
 end
 
 
 %% Plot
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-figure('position',[10,10,1200,1200]); hold on; axis off; rotate3d on;
+figure('position',[10,10,1200,1200]); hold on; axis on; box on; rotate3d on;
+% colormap(repmat(linspace(1,0,64),3,1)');
+colormap hsv;
+
 plotGMM3D(Mu, Sigma, [.2 .2 .2], .3, 2);
+
+% [x,y,z] = meshgrid(xm1d, xm1d, xm1d);
+% v = reshape(g,[nbRes,nbRes,nbRes]);
+% v = permute(v,[2,1,3]);
+% v(v<1E-3) = nan; 
+% % pcolor3(x,y,z,v,'alpha',.1);
+% % vol3d('XData',x,'YData',y,'ZData',z,'CData',v);
+% h = slice(x, y, z, v, [], [], xlim(1):.02:xlim(2));
+% set(h,'edgecolor','none','facecolor','interp','facealpha','interp');
+% alpha('color');
+% % alphamap('rampdown')
+% % alphamap('increase',.05)
+
 plot3(Mu(1,:), Mu(2,:), Mu(3,:), '.','markersize',15,'color',[0 0 0]);
 plot3(r.x(1,:), r.x(2,:), r.x(3,:), '-','linewidth',1,'color',[0 0 0]);
 plot3(r.x(1,1), r.x(2,1), r.x(3,1), '.','markersize',15,'color',[0 0 0]);
-axis([xlim(1),xlim(2),xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; axis vis3d; view(60,25);
-% plot3Dframe(eye(3).*.3, ones(3,1).*.2);
+plot3Dframe(eye(3).*.3, ones(3,1).*.2);
+axis([xlim(1),xlim(2),xlim(1),xlim(2),xlim(1),xlim(2)]); axis equal; axis vis3d; view(50,20);
+set(gca,'xtick',[],'ytick',[],'ztick',[]);
 % print('-dpng','graphs/ergodicControl_3D01.png'); 
 
 pause;
diff --git a/demos/demo_ergodicControl_nD01.m b/demos/demo_ergodicControl_nD01.m
index 5ee444689307cbf6ce93dc73b95a0fdddda77ec4..6fcbfbeb79df9b28de11f7637a4d8c07c084bc76 100644
--- a/demos/demo_ergodicControl_nD01.m
+++ b/demos/demo_ergodicControl_nD01.m
@@ -37,7 +37,7 @@ addpath('./m_fcts/');
 %% Parameters
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 nbData = 2000; %Number of datapoints
-nbFct = 8; %Number of basis functions along x and y
+nbFct = 5; %Number of basis functions along x and y
 nbVar = 4; %Dimension of datapoint
 nbStates = 2; %Number of Gaussians to represent the spatial distribution
 sp = (nbVar + 1) / 2; %Sobolev norm parameter
@@ -46,7 +46,6 @@ xlim = [0; 1]; %Domain limit for each dimension (considered to be 1 for each dim
 L = (xlim(2) - xlim(1)) * 2; %Size of [-xlim(2),xlim(2)]
 om = 2 .* pi ./ L; %omega
 u_max = 5E1; %Maximum speed allowed 
-x = rand(nbVar,1); %Initial position
 
 %Desired spatial distribution represented as a mixture of Gaussians
 Mu = rand(nbVar,nbStates);
@@ -68,12 +67,12 @@ for n=1:nbVar
 end
 Lambda = (stmp + 1).^-sp; %Weighting vector (Eq.(15))
 
-HK = L^nbVar; %Rescaling term (as scalar)
-% hk = [1; sqrt(.5)*ones(nbFct-1,1)];
-% HK = ones(nbFct^nbVar, 1);
-% for n=1:nbVar
-% 	HK = HK .* hk(arr(n).x(:),1); %Rescaling term (as normalizing matrix)
-% end
+% HK = L^nbVar; %Rescaling term (as scalar)
+% % hk = [1; sqrt(.5)*ones(nbFct-1,1)];
+% % HK = ones(nbFct^nbVar, 1);
+% % for n=1:nbVar
+% % 	HK = HK .* hk(arr(n).x(:),1); %Rescaling term (as normalizing matrix)
+% % end
 
 %Explicit description of phi_k by exploiting the Fourier transform properties of Gaussians (optimized version by exploiting symmetries)
 %Enumerate symmetry operations for 2D signal ([-1,-1],[-1,1],[1,-1] and [1,1]), and removing redundant ones -> keeping ([-1,-1],[-1,1])
@@ -92,11 +91,12 @@ for j=1:nbStates
 		w_hat = w_hat + Priors(j) .* cos(kk' * MuTmp) .* exp(diag(-.5 * kk' * SigmaTmp * kk)); %Eq.(21)
 	end
 end
-w_hat = w_hat ./ HK ./ size(op,2);
+w_hat = w_hat ./ L^nbVar ./ size(op,2);
 
 
 %% Ergodic control 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+x = rand(nbVar,1); %Initial position
 wt = zeros(nbFct^nbVar, 1);
 for t=1:nbData
 	r.x(:,t) = x; %Log data
@@ -105,10 +105,6 @@ for t=1:nbData
 	phi1 = cos(x * rg .* om); %Eq.(18)
 	dphi1 = -sin(x * rg .* om) .* repmat(rg,nbVar,1) .* om;
 	
-% 	%Fourier basis functions and derivatives for each dimension (only real part on [0,L/2] is computed since the signal is even and real by construction) 
-% 	phi1 = real(exp(-i .* x * rg .* om)); %Eq.(18)
-% 	dphi1 = real(-exp(-i .* x * rg .* om) .* i .* repmat(rg,nbVar,1) .* om);
-
 	dphi = ones(nbVar, nbFct^nbVar);
 	phi = ones(nbFct^nbVar, 1);
 	for n=1:nbVar
@@ -121,7 +117,7 @@ for t=1:nbData
 		end
 		phi = phi .* phi1(n,arr(n).x(:))';
 	end	
-	wt = wt + phi ./ HK;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
+	wt = wt + phi ./ L^nbVar;	%wt./t are the Fourier series coefficients along trajectory (Eq.(17))
 	
 % 	%Controller with ridge regression formulation
 % 	u = -dphi * (Lambda .* (wt./t - w_hat)) .* t .* 1E-1; %Velocity command