diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index cc9f65be15cadcb21aebf8f9505dbf4931e54008..d14dc5b13603018c72d3c4a5f901bd9fc9348677 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -16,8 +16,8 @@ class UBloxRaw
 
   RawDataType addDataStream(const std::vector<u_int8_t>& data_stream);
   
-  Observations& getObservations();
-  Navigation& getNavigation();
+  const Observations& getObservations();
+  const Navigation& getNavigation();
 
   RawDataType getRawDataType() const;
 
@@ -33,12 +33,12 @@ class UBloxRaw
 
 };
 
-inline GNSSUtils::Observations& UBloxRaw::getObservations()
+inline const GNSSUtils::Observations& UBloxRaw::getObservations()
 {
     return obs_;
 }
 
-inline GNSSUtils::Navigation& UBloxRaw::getNavigation()
+inline const GNSSUtils::Navigation& UBloxRaw::getNavigation()
 {
     return nav_;
 }
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 9912535c39f9597ceea9acf0d7dc5c02629e7f73..6747ab860d2cbd80bc83ff28b06378d0c45323c4 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -30,11 +30,12 @@ namespace GNSSUtils
     output.time = sol.time.time;
     output.sec = sol.time.sec;
     output.pos  = Eigen::Vector3d(sol.rr);
-    std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
     output.vel  = Eigen::Vector3d(&sol.rr[3]);
     output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
                         sol.qr[3], sol.qr[1], sol.qr[4],
-                        sol.qr[5], sol.qr[3], sol.qr[2];
+                        sol.qr[5], sol.qr[4], sol.qr[2];
+    //std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
+    //std::cout << "Covariance:\n" << output.pos_covar << "\n";
 
     // XXX: segmentation fault here.
     // if (sol.dtr != NULL)
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
index 8fdc8cbfa8f09ff7b71f65a5c245c8edeb1cf297..ab7b7399edbf49e24d87177d5cf404fed70a36ec 100644
--- a/src/ublox_raw.cpp
+++ b/src/ublox_raw.cpp
@@ -31,7 +31,7 @@ RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
                 break;
 
             case OBS: // Observations
-                //std::cout << "Observations received!\n";
+                std::cout << "Observations received!\n";
                 updateObservations();
                 //std::cout << "Observations updated!\n";
                 break;