diff --git a/CMakeLists.txt b/CMakeLists.txt
index b3b349ca4d358ff10d3c78a809e4bd08b645142d..efa38b3cadd903b97d29408bab5be4af971acf15 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,13 @@ ENDIF(WIN32)
 
 # --- OPTIONS ----------------------------------------
 OPTION (BUILD_BENCHMARK "Build the benchmarks" OFF)
+OPTION (INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
+
+IF (INITIALIZE_WITH_NAN)
+  MESSAGE (STATUS "Initialize with NaN all the Eigen entries")
+  ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
+  #LIST(APPEND CMAKE_CXX_FLAGS "-DEIGEN_INITIALIZE_MATRICES_BY_NAN")
+ENDIF (INITIALIZE_WITH_NAN)
 
 # ----------------------------------------------------
 # --- DEPENDANCIES -----------------------------------
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index da14e21789940fcce907c79bfef84e042aa7a5d7..a2ace9e4d5120319736c251836c62a91c7affc1c 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -347,12 +347,12 @@ namespace se3
     ,lowerPositionLimit(ref.nq)
     ,upperPositionLimit(ref.nq)
   {
+    /* Create data strcture associated to the joints */
     for(Model::Index i=0;i<(Model::Index)(model.nbody);++i) 
       joints.push_back(CreateJointData::run(model.joints[i]));
 
     /* Init for CRBA */
-    M.fill(NAN);    
-    for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); Fcrb[i].fill(NAN); }
+    for(Model::Index i=0;i<(Model::Index)(ref.nbody);++i ) { Fcrb[i].resize(6,model.nv); }
     computeLastChild(ref);
 
     /* Init for Cholesky */
@@ -361,6 +361,14 @@ namespace se3
 
     /* Init Jacobian */
     J.fill(0);
+    
+    /* Init universe states relatively to itself */
+    
+    a[0].setZero();
+    v[0].setZero();
+    f[0].setZero();
+    oMi[0].setIdentity();
+    liMi[0].setIdentity();
   }
 
   inline void Data::computeLastChild(const Model& model)