diff --git a/crosbot_fastslam/include/crosbot_fastslam/module.hpp b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
index 833af1ee91536455a14d4d6996026654a17a28aa..675aeda3cd4948ec3357c50c3d03f7fafd341d30 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/module.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/module.hpp
@@ -120,7 +120,23 @@ public:
 			}
 		}
 
-		// TODO: publish history
+		// publish history
+		if (historyPub.getNumSubscribers() > 0) {
+			nav_msgs::PathPtr rosPath(new nav_msgs::Path());
+			rosPath->header.frame_id = mapFrame;
+			rosPath->header.stamp = newMean->getLatestUpdate()->timestamp.toROS();
+
+			PathPtr path = newMean->getPath();
+			rosPath->poses.resize(path->path.size());
+			for (size_t i = 0; i < path->path.size(); ++i) {
+				geometry_msgs::PoseStamped& pose = rosPath->poses[i];
+				pose.pose = path->path[i].toROS();
+				pose.header.frame_id = mapFrame;
+				pose.header.stamp = (i < path->timestamps.size())?(path->timestamps[i].toROS()):ros::Time(0);
+			}
+
+			historyPub.publish(rosPath);
+		}
 	}
 
 	void tagAdded(MapPtr map, TagPtr tag) {
diff --git a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp
index 05b7274f08d2f90a570421fa228927a3cd559a41..a1fa2d2270c175a4784ac580eba49a11f2c730b3 100644
--- a/crosbot_fastslam/include/crosbot_fastslam/particle.hpp
+++ b/crosbot_fastslam/include/crosbot_fastslam/particle.hpp
@@ -61,6 +61,10 @@ public:
 
 		for (size_t i = 0; i < history.size(); i++) {
 			rval->path.push_back(history[i].pose);
+			if (history[i].cloud != NULL)
+				rval->timestamps.push_back(history[i].cloud->timestamp);
+			else
+				rval->timestamps.push_back(Time(0,0));
 		}
 
 		if (rval->path.size() == 0 || rval->path[rval->path.size()-1] != pose)
diff --git a/crosbot_fastslam/src/fastslam.cpp b/crosbot_fastslam/src/fastslam.cpp
index f4d88746b12c00f4ab6bdf274ca62b549caf8259..47c56dded51cfb528f63b5900441674db16da825 100644
--- a/crosbot_fastslam/src/fastslam.cpp
+++ b/crosbot_fastslam/src/fastslam.cpp
@@ -468,7 +468,8 @@ void FastSLAMMap::moveParticles(Pose relativeMotion, MapCloudPtr cloud, bool cal
 
 	for (size_t i = 0; i < jobs.size(); i++) {
 		delete jobs[i];
-	} jobs.clear();
+	}
+	jobs.clear();
 }
 
 void FastSLAMMap::updateParticles(MapCloudPtr cloud) {
diff --git a/crosbot_map/include/crosbot_map/map.hpp b/crosbot_map/include/crosbot_map/map.hpp
index dc69d4be56696bf37c5d8ff09d345ce22fdb2366..b0fa5c31a9e3c0a55a688e2c72db8c7c1a834885 100644
--- a/crosbot_map/include/crosbot_map/map.hpp
+++ b/crosbot_map/include/crosbot_map/map.hpp
@@ -20,9 +20,10 @@ namespace crosbot {
 class Path : public HandledObject {
 public:
 	std::vector<Pose> path;
+	std::vector<Time> timestamps;
 
 	Path() {}
-	Path(Pose pose) {
+	Path(Pose pose, Time timstamp = Time(0,0)) {
 		path.push_back(pose);
 	}
 };