diff --git a/crosbot_explore/src/explorerROS.cpp b/crosbot_explore/src/explorerROS.cpp
index 4160aa092093ce81f4a72b9b1f53fcd6ae7f41ad..f2b2e7ca4d57e85f5db03ff39ec5d1ef5857b4c1 100644
--- a/crosbot_explore/src/explorerROS.cpp
+++ b/crosbot_explore/src/explorerROS.cpp
@@ -345,19 +345,18 @@ void ExplorerROSNode::loadLatestPose() {
 Pose ExplorerROSNode::getLatestPose(std::string frame_id, Time timestamp) {
     //if (debugMsgs) ROS_INFO("getLatestPose() - %s, %s at time: %.5lf", frame_id.c_str(), baseFrame.c_str(), timestamp.toSec());
 
+
     tf::StampedTransform transform;
-<<<<<<< Updated upstream
-=======
     bool loaded = false;
     ros::Time tNow = ros::Time::now();
 
     // TODO: Temporary
     timestamp = tNow;// - ros::Duration(1.0);
 
->>>>>>> Stashed changes
     try {
         tfListener.waitForTransform(frame_id, baseFrame, timestamp.toROS(), ros::Duration(2.0));
         tfListener.lookupTransform(frame_id, baseFrame, timestamp.toROS(), transform);
+        loaded = true;
     } catch (tf::TransformException& e) {
         ROS_WARN("%s getLatestPose(): Exception caught(%s). time: %.5lf, %s - %s\n", LOG_START, e.what(), timestamp.toSec(), frame_id.c_str(), baseFrame.c_str());
         return Pose(INFINITY, INFINITY, INFINITY);
diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp
index 410436ad117866fd030a2ce6c7388dcc5a36b3e8..93567547cdfbb7d942e3a97dbe33d2bd1601c0c0 100644
--- a/crosbot_fastslam/src/fastslam.cpp
+++ b/crosbot_fastslam/src/fastslam.cpp
@@ -437,11 +437,11 @@ void FastSLAMMap::insertTag(TagPtr tag) {
 	if (mean == NULL) {
 		// No mean particle so create one
 		mean = new Particle(parameters);
-		mean->addTag(tag, true);
+		mean->addTag(tag, false);
 		return;
 	}
 
-	mean->addTag(tag, true);
+	mean->addTag(tag, false);
 	motion->addTag(tag);
 	Lock lock(particlesLock, true);
 	for (size_t i = 0; i < particles.size(); i++) {
diff --git a/crosbot_fastslam/src/module.cpp b/crosbot_fastslam/src/module.cpp
index 47254474987b1145d8371fd94e03fdbc6102b676..0bd5df38d9774232339bc20d480884dd4ade6431 100644
--- a/crosbot_fastslam/src/module.cpp
+++ b/crosbot_fastslam/src/module.cpp
@@ -184,11 +184,17 @@ void FastSLAMModule::callbackSnap(const crosbot_map::SnapMsgConstPtr& snap) {
 
 bool FastSLAMModule::service_listSnaps(crosbot_map::ListSnapsRequest &request, crosbot_map::ListSnapsResponse &response) {
     // Iterate through snaps - adding to response
-    ROS_INFO("%s listing Snaps", LOG_START);
+    //ROS_INFO("%s listing Snaps", LOG_START);
     TagListPtr tagList = getTags();
     for (TagLocation& tagLoc : tagList->tags) {
+        //ROS_INFO("%s getting tag: %d at robot %s pose %s", LOG_START, tagLoc.tag->id, tagLoc.tag->robot.toString().c_str(), tagLoc.tag->pose.toString().c_str());
+
         Snap* snap = dynamic_cast<Snap*>(tagLoc.tag.get());
         crosbot_map::SnapMsgPtr snapMsg = snap->toROS();
+        // Need to set actual robot and pose locations (snap does not keep these)
+        snapMsg->robot = tagLoc.robot.toROS();
+        snapMsg->pose = tagLoc.mapPose.toROS();
+
         response.snaps.push_back(*snapMsg);
     }
 
diff --git a/crosbot_fastslam/src/particle.cpp b/crosbot_fastslam/src/particle.cpp
index 1c4509ecb8a27da03945a315232dff45e0620269..1a951c3360ca7664123fe3bfaac88f37cbb30e94 100644
--- a/crosbot_fastslam/src/particle.cpp
+++ b/crosbot_fastslam/src/particle.cpp
@@ -9,6 +9,8 @@
 
 #include <crosbot_fastslam/particle.hpp>
 
+#define LOG_START       "Particle ::"
+
 namespace crosbot {
 
 namespace fastslam {
@@ -48,10 +50,11 @@ void Particle::addTag(TagPtr tag, bool checkForDuplicate) {
 
 	// Calculate tag & robot poses
 	tf::Transform robotTrans =
-			pose.toTF() * trackerPose.toTF().inverse() *
+			pose.toTF() * //trackerPose.toTF().inverse() * TODO: WHY IS THE TRACKER POSE HERE?
 			tag->robot.toTF();
 	robotPose = robotTrans;
 	tagPose = robotTrans * tag->pose.toTF();
+	ROS_INFO("%s Adding tag at robot: %s, tag: %s (this pose: %s)", LOG_START, robotPose.toString().c_str(), tagPose.toString().c_str(), pose.toString().c_str());
 
 	if (checkForDuplicate) {
 		// check if the tag is a duplicate snap
@@ -61,8 +64,9 @@ void Particle::addTag(TagPtr tag, bool checkForDuplicate) {
 				Map::TagLocation& tl = tags->tags[i];
 				SnapPtr snap2 = dynamic_cast<Snap*>(tl.tag.get());
 				if (snap2 == NULL || snap->type != snap2->type ||
-						snap2->status == Snap::REJECTED || snap2->status == Snap::DUPLICATE)
+						snap2->status == Snap::REJECTED || snap2->status == Snap::DUPLICATE) {
 					continue;
+				}
 
 				if (tl.mapPose.position.distanceTo(tagPose.position) < parameters.minDistanceBetweenSnaps) {
 					if (snap2->status == Snap::UNCONFIRMED &&
@@ -77,8 +81,22 @@ void Particle::addTag(TagPtr tag, bool checkForDuplicate) {
 		}
 	}
 
+
 	Lock lock(rwLock, true);
-	tags = new Map::TagList(tags);
+	// Check for existing tag by id - if found, then update existing tag, otherwise add new tag
+	bool existing = false;
+	//tags = new Map::TagList(tags);
+	Map::TagListPtr oldTags = tags;
+	tags = new Map::TagList();
+	for (Map::TagLocation& it : oldTags->tags) {
+	    if (it.tag->id != tag->id) {
+	        // Update
+	        //ROS_INFO("%s duplicate tag - updating tagloc instead", LOG_START);
+	        tags->tags.push_back(it);
+	    } else {
+	        ROS_INFO("%s duplicate tag - updating tagloc instead", LOG_START);
+	    }
+	}
 	tags->tags.push_back(Map::TagLocation(tag, robotPose, tagPose));
 }