diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 646dd912149c8c523bba954dd4bd8e435ac0b8a8..197232cf32167a0cd5fd1fd0fdb8252bb94d20b3 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -4,6 +4,7 @@
 #include <vector>
 #include <iostream>
 #include <memory>
+#include <string>
 
 #include <eigen3/Eigen/Dense>
 #include <eigen3/Eigen/Geometry>
@@ -17,6 +18,8 @@ extern "C"
     #include "rtklib.h"
 }
 
+#define GNSSUTILS_MSG "--GNSSUtils--"
+
 namespace GNSSUtils
 {
   struct ComputePosOutput{
@@ -37,10 +40,17 @@ namespace GNSSUtils
     Eigen::Vector3d lat_lon;     // latitude_longitude_altitude
   };
 
-  GNSSUtils::ComputePosOutput computePos(const Observations & _observations,
+  ComputePosOutput computePos(const Observations & _observations,
                                     Navigation & _navigation,
                                     const prcopt_t & _prcopt);
   
   Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
+
+  void print(std::string & _msg);
+  void printArray(std::string _name, int * _array, int size);
+  void printArray(std::string _name, unsigned char * _array, int size);
+  void printArray(std::string _name, double * _array, int size);
+  void printArray(std::string _name, float * _array, int size);
 }
+
 #endif
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 43a3e21b554a78e5e5580b004a22d1b429738987..b1e6ab26d22dcbfa7a35578dc39f142c6c44f9a9 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -28,12 +28,16 @@ namespace GNSSUtils
 
       void clearObservations();
 
-      void pushObservation(obsd_t obs);
+      void pushObservation(obsd_t & obs);
 
       void reserveObservations(unsigned int n);
 
       const std::vector<obsd_t> & getObservations() const;
 
+      void print(obsd_t & _obs);
+      void print(int _obs_idx);
+      void print();
+
 
 
     private:
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index 569d1448d6cf5b8451f2b8acae56108cf24e825f..fd5589b48b65ae24c960a19b75900cd8c419cc00 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -128,10 +128,6 @@ int createObsAndNav(Observations* observations, char* obs_path,  Navigation* nav
   return 1;
 }
 
-
-
-
-
 int main(int argc, char *argv[])
 {
   /* header */
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 439e81bbea321ae2c5c07f51f334e2e09898f77a..a976c3b2ec1f55e87e63bcc14c0397b6f2485c7a 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -3,30 +3,15 @@
 
 namespace GNSSUtils
 {
-  GNSSUtils::ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
+  
+  ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
                                     GNSSUtils::Navigation & _navigation,
                                     const prcopt_t & _prcopt)
   {
+
     // Remove duplicated satellites
     uniqnav(&(_navigation.getNavigation()));
 
-    std::cout << "Number of navigation satellites: " << _navigation.getNavigation().n << "\n";
-    std::cout << "Number of observations: " << _observations.getObservations().size() << "\n";
-
-    // Define processing options
-    prcopt_t prcopt = prcopt_default;
-    prcopt.mode = PMODE_SINGLE;
-    prcopt.soltype = 0;
-    prcopt.nf = 1;
-    prcopt.navsys = SYS_GPS;
-    // prcopt.elmin = 1.05;  // 60 degrees = 1.05 rad
-    prcopt.sateph = EPHOPT_BRDC;
-    prcopt.ionoopt = IONOOPT_OFF;
-    prcopt.tropopt = TROPOPT_OFF;
-    prcopt.dynamics = 0;
-    prcopt.tidecorr = 0;
-    prcopt.sbascorr = SBSOPT_FCORR;
-
     // Define error msg
     char msg[128] = "";
 
@@ -36,7 +21,7 @@ namespace GNSSUtils
 
     output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(),
                             &(_navigation.getNavigation()),
-                            &prcopt, &sol, NULL, NULL, msg);
+                            &_prcopt, &sol, NULL, NULL, msg);
     
     if (output.pos_stat == 0)
     {
@@ -46,7 +31,7 @@ namespace GNSSUtils
     output.time = sol.time.time;
     output.time = sol.time.sec;
     output.pos  = Eigen::Vector3d(sol.rr);
-    std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
+    // 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],
@@ -75,4 +60,64 @@ namespace GNSSUtils
 
     return Eigen::Vector3d(pos);
   }
+
+  void print(std::string & _msg) 
+  {
+    std::string msg = GNSSUTILS_MSG + _msg;
+
+    std::cout << msg << "\n";
+     
+  }
+
+  void printArray(std::string _name, int * _array, int size)
+  {  
+    std::cout << _name << ": [";
+    for (int ii=0; ii<size; ++ii)
+    {
+      std::cout << _array[ii];
+      if (ii==size-1) 
+        std::cout << "] \n";
+      else
+        std::cout << ",";
+    }
+  }
+
+  void printArray(std::string _name, unsigned char * _array, int size)
+  {
+    std::cout << _name << ": [";
+    for (int ii=0; ii<size; ++ii)
+    {
+      std::cout << (unsigned)_array[ii];
+      if (ii==size-1) 
+        std::cout << "] \n";
+      else
+        std::cout << ",";
+    }
+  }
+
+  void printArray(std::string _name, double * _array, int size)
+  {   
+    std::cout << _name << ": [";
+    for (int ii=0; ii<size; ++ii)
+    {
+      std::cout << _array[ii];
+      if (ii==size-1) 
+        std::cout << "] \n";
+      else
+        std::cout << ",";
+    }
+  }
+
+  void printArray(std::string _name, float * _array, int size)
+  {   
+    std::cout << _name << ": [";
+    for (int ii=0; ii<size; ++ii)
+    {
+      std::cout << _array[ii];
+      if (ii==size-1) 
+        std::cout << "] \n";
+      else
+        std::cout << ",";
+    }
+  }
 }
diff --git a/src/observations.cpp b/src/observations.cpp
index 87c6eea9bb6b4840716289a6d1e50a6862ef8a5f..9892903180717c65a832ca35dda5cdc428a6fd20 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,5 +1,7 @@
+#include "gnss_utils/gnss_utils.h"
 #include "gnss_utils/observations.h"
 
+
 using namespace GNSSUtils;
 
 
@@ -18,7 +20,7 @@ void Observations::clearObservations()
   this->obs_vector_.clear();
 }
 
-void Observations::pushObservation(obsd_t obs)
+void Observations::pushObservation(obsd_t & obs)
 {
   this->obs_vector_.push_back(obs);
 }
@@ -32,3 +34,29 @@ const std::vector<obsd_t> & Observations::getObservations() const
 {
   return this->obs_vector_;
 }
+
+void Observations::print(obsd_t & _obs)
+{
+  std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
+  GNSSUtils::print(msg);
+  std::cout << "Time [s]: " << _obs.time.time << "\n";
+  printArray("SNR: ", _obs.SNR, sizeof(_obs.SNR) / sizeof(_obs.SNR[0]));
+  printArray("LLI: ", _obs.LLI, sizeof(_obs.LLI) / sizeof(_obs.LLI[0]));
+  printArray("code: ", _obs.code, sizeof(_obs.code) / sizeof(_obs.code[0]));
+  printArray("L: ", _obs.L, sizeof(_obs.L) / sizeof(_obs.L));
+  printArray("P: ", _obs.P, sizeof(_obs.P) / sizeof(_obs.P));
+  printArray("D: ", _obs.D, sizeof(_obs.D) / sizeof(_obs.D));
+}
+
+void Observations::print(int _obs_idx)
+{
+  print(obs_vector_[_obs_idx]);
+}
+
+void Observations::print()
+{
+  for (auto obs: obs_vector_)
+  {
+    print(obs);
+  }
+}
\ No newline at end of file