diff --git a/inekf_ros/include/Measurement.h b/inekf_ros/include/Measurement.h
index de7f041..d6fd741 100644
--- a/inekf_ros/include/Measurement.h
+++ b/inekf_ros/include/Measurement.h
@@ -59,8 +59,6 @@ class ImuMeasurement : public Measurement {
 
     private: 
         Eigen::Matrix<double,6,1> data_;
-
-        //friend std::ostream& operator<<(std::ostream& os, const ImuMeasurement& m);  
 };
 
 
@@ -69,11 +67,10 @@ class LandmarkMeasurement : public Measurement {
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         LandmarkMeasurement(const inekf_msgs::LandmarkArray::ConstPtr& msg, const tf::StampedTransform& transform);
-        inekf::vectorPairIntVector3d getData();
-        //friend std::ostream& operator<<(std::ostream& os, const Measurement& m);  
+        inekf::vectorLandmarks getData();
 
     private:
-        inekf::vectorPairIntVector3d data_;
+        inekf::vectorLandmarks data_;
 };
 
 class ContactMeasurement : public Measurement {
@@ -82,7 +79,6 @@ class ContactMeasurement : public Measurement {
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         ContactMeasurement(const inekf_msgs::ContactArray::ConstPtr& msg);
         std::vector<std::pair<int,bool>> getData();
-        //friend std::ostream& operator<<(std::ostream& os, const Measurement& m);  
 
     private:
         std::vector<std::pair<int,bool>> data_;
@@ -94,11 +90,10 @@ class KinematicMeasurement : public Measurement {
     public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         KinematicMeasurement(const inekf_msgs::KinematicsArray::ConstPtr& msg);
-        inekf::vectorTupleIntMatrix4dMatrix6d getData();
-        //friend std::ostream& operator<<(std::ostream& os, const Measurement& m);  
+        inekf::vectorKinematics getData();
 
     private:
-        inekf::vectorTupleIntMatrix4dMatrix6d data_;
+        inekf::vectorKinematics data_;
 };
 
 
diff --git a/inekf_ros/src/InEKF_ROS.cpp b/inekf_ros/src/InEKF_ROS.cpp
index 1991c6e..b29121f 100644
--- a/inekf_ros/src/InEKF_ROS.cpp
+++ b/inekf_ros/src/InEKF_ROS.cpp
@@ -364,12 +364,12 @@ void InEKF_ROS::publishLandmarkMeasurementMarkers(shared_ptr<LandmarkMeasurement
     base_point.y = position(1);
     base_point.z = position(2);
 
-    vectorPairIntVector3d measured_landmarks = ptr->getData();
+    vectorLandmarks measured_landmarks = ptr->getData();
     mapIntVector3d prior_landmarks = filter_.getPriorLandmarks();
     map<int,int> estimated_landmarks = filter_.getEstimatedLandmarks();
     for (auto it=measured_landmarks.begin(); it!=measured_landmarks.end(); ++it) {
         // Search through prior landmarks
-        auto search_prior = prior_landmarks.find(it->first);
+        auto search_prior = prior_landmarks.find(it->id);
         if (search_prior != prior_landmarks.end()) {
             landmark_point.x = search_prior->second(0);
             landmark_point.y = search_prior->second(1);
@@ -379,7 +379,7 @@ void InEKF_ROS::publishLandmarkMeasurementMarkers(shared_ptr<LandmarkMeasurement
             continue;
         }
         // Search through estimated landmarks
-        auto search_estimated = estimated_landmarks.find(it->first);
+        auto search_estimated = estimated_landmarks.find(it->id);
         if (search_estimated != estimated_landmarks.end()) {
             landmark_point.x = X(0,search_estimated->second);
             landmark_point.y = X(1,search_estimated->second);
@@ -421,11 +421,11 @@ void InEKF_ROS::publishKinematicMeasurementMarkers(shared_ptr<KinematicMeasureme
     base_point.y = position(1);
     base_point.z = position(2);
 
-    vectorTupleIntMatrix4dMatrix6d measured_kinematics = ptr->getData();
+    vectorKinematics measured_kinematics = ptr->getData();
     map<int,int> estimated_contacts = filter_.getEstimatedContactPositions();
     for (auto it=measured_kinematics.begin(); it!=measured_kinematics.end(); ++it) {
         // Search through estimated contacts
-        auto search_estimated = estimated_contacts.find(get<0>(*it));
+        auto search_estimated = estimated_contacts.find(it->id);
         if (search_estimated != estimated_contacts.end()) {
             contact_point.x = X(0,search_estimated->second);
             contact_point.y = X(1,search_estimated->second);
diff --git a/inekf_ros/src/Measurement.cpp b/inekf_ros/src/Measurement.cpp
index 8f74f06..2bdf16c 100644
--- a/inekf_ros/src/Measurement.cpp
+++ b/inekf_ros/src/Measurement.cpp
@@ -46,10 +46,10 @@ LandmarkMeasurement::LandmarkMeasurement(const inekf_msgs::LandmarkArray::ConstP
         tf::Vector3 p_bl = transform*p_cl; // Transform measurement from camera frame to imu frame
         Eigen::Vector3d position;
         position << p_bl.getX(), p_bl.getY(), p_bl.getZ();
-        data_.push_back(pair<int,Eigen::Vector3d> (it->id, position)); 
+        data_.push_back(Landmark(it->id, position)); 
     }
 }
-vectorPairIntVector3d LandmarkMeasurement::getData() { return data_; }; 
+vectorLandmarks LandmarkMeasurement::getData() { return data_; }; 
 
 
 // Construct Contact measurement
@@ -80,10 +80,10 @@ KinematicMeasurement::KinematicMeasurement(const inekf_msgs::KinematicsArray::Co
                 covariance(i,j) = it->pose.covariance[6*i+j]; // Assume row-major
             }
         }
-        data_.push_back(tuple<int,Eigen::Matrix4d,Eigen::Matrix<double,6,6>> (it->id, pose, covariance));
+        data_.push_back(Kinematics(it->id, pose, covariance));
     }
 }
-vectorTupleIntMatrix4dMatrix6d KinematicMeasurement::getData() { return data_; }; 
+vectorKinematics KinematicMeasurement::getData() { return data_; }; 
 
 // Print measurement
 ostream& operator<<(ostream& os, const Measurement& m) {
@@ -96,6 +96,5 @@ ostream& operator<<(ostream& os, const Measurement& m) {
             type_str = "Unknown";
     }
     os << "Measurement type: " << type_str << endl;
-    //os << "Measurement time: " << m.t_ << endl;
-    //os << "Measurement data: \n" << m.data_ << endl;
+    return os;
 }