diff --git a/include/rf2o_laser_odometry/CLaserOdometry2D.h b/include/rf2o_laser_odometry/CLaserOdometry2D.h
index d01b86d..6985391 100644
--- a/include/rf2o_laser_odometry/CLaserOdometry2D.h
+++ b/include/rf2o_laser_odometry/CLaserOdometry2D.h
@@ -67,9 +67,10 @@ public:
 
     void setLaserPose(const mrpt::poses::CPose3D& laser_pose);
 
-    mrpt::poses::CPose3D& getIncrement();
     const mrpt::poses::CPose3D& getIncrement() const;
 
+    const Eigen::Matrix<float, 3, 3>& getIncrementCovariance() const;
+
     mrpt::poses::CPose3D& getPose();
     const mrpt::poses::CPose3D& getPose() const;
 
diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp
index ad4b1e1..12c4c5b 100644
--- a/src/CLaserOdometry2D.cpp
+++ b/src/CLaserOdometry2D.cpp
@@ -158,15 +158,14 @@ void CLaserOdometry2D::Init(const sensor_msgs::LaserScan& scan,
     last_odom_time = ros::Time::now();
 }
 
-
-mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement()
+const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const
 {
   return last_increment;
 }
 
-const mrpt::poses::CPose3D& CLaserOdometry2D::getIncrement() const
+const Eigen::Matrix<float, 3, 3>& CLaserOdometry2D::getIncrementCovariance() const
 {
-  return last_increment;
+  return cov_odo;
 }
 
 mrpt::poses::CPose3D& CLaserOdometry2D::getPose()