diff --git a/ruvu_mcl/CMakeLists.txt b/ruvu_mcl/CMakeLists.txt
index a34b602..aaf0134 100644
--- a/ruvu_mcl/CMakeLists.txt
+++ b/ruvu_mcl/CMakeLists.txt
@@ -92,8 +92,11 @@ add_library(${PROJECT_NAME}
 target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
 )
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+# bug in erf_inv_initializer under valgrind
+# https://svn.boost.org/trac10/ticket/10005
+target_compile_definitions(${PROJECT_NAME} PRIVATE BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS)
 target_compile_warnings(${PROJECT_NAME} ${EXTRA_FLAGS})
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
 
 add_executable(node
   src/main.cpp
diff --git a/ruvu_mcl/src/cloud_publisher.cpp b/ruvu_mcl/src/cloud_publisher.cpp
index 7c96abd..7a1c84f 100644
--- a/ruvu_mcl/src/cloud_publisher.cpp
+++ b/ruvu_mcl/src/cloud_publisher.cpp
@@ -4,6 +4,7 @@
 
 #include <algorithm>
 #include <utility>
+#include <vector>
 
 #include "./particle_filter.hpp"
 #include "ros/node_handle.h"
@@ -17,7 +18,7 @@ CloudPublisher::CloudPublisher(ros::NodeHandle nh, ros::NodeHandle private_nh)
 {
 }
 
-void CloudPublisher::publish(const std_msgs::Header & header, const ParticleFilter & pf)
+void CloudPublisher::publish(const std_msgs::Header & header, const std::vector<Particle> & pf)
 {
   constexpr double length = 0.1;
   constexpr double width = 0.02;
@@ -30,14 +31,14 @@ void CloudPublisher::publish(const std_msgs::Header & header, const ParticleFilt
   m.action = visualization_msgs::Marker::MODIFY;
   m.pose.orientation = tf2::toMsg(tf2::Quaternion::getIdentity());
   m.scale.x = width;
-  m.points.reserve(pf.particles.size() * 2);
-  m.colors.reserve(pf.particles.size() * 2);
+  m.points.reserve(pf.size() * 2);
+  m.colors.reserve(pf.size() * 2);
 
   double max_weight = 0;
-  for (const auto & p : pf.particles) max_weight = std::max(max_weight, p.weight);
+  for (const auto & p : pf) max_weight = std::max(max_weight, p.weight);
 
   tf2::Vector3 p1{length / 2, 0, 0};
-  for (const auto & particle : pf.particles) {
+  for (const auto & particle : pf) {
     std_msgs::ColorRGBA c;
     c.a = particle.weight / max_weight;
     c.b = 1;
diff --git a/ruvu_mcl/src/cloud_publisher.hpp b/ruvu_mcl/src/cloud_publisher.hpp
index a6a924d..40e5529 100644
--- a/ruvu_mcl/src/cloud_publisher.hpp
+++ b/ruvu_mcl/src/cloud_publisher.hpp
@@ -2,12 +2,14 @@
 
 #pragma once
 
+#include <vector>
+
 #include "ros/message_forward.h"
 #include "ros/publisher.h"
 
 namespace ros
 {
-class NodeHandle;
+class Particle;
 }
 namespace std_msgs
 {
@@ -16,13 +18,13 @@ ROS_DECLARE_MESSAGE(Header)
 
 namespace ruvu_mcl
 {
-class ParticleFilter;
+class Particle;
 
 class CloudPublisher
 {
 public:
   CloudPublisher(ros::NodeHandle nh, ros::NodeHandle private_nh);
-  void publish(const std_msgs::Header & header, const ParticleFilter & pf);
+  void publish(const std_msgs::Header & header, const std::vector<Particle> & pf);
 
 private:
   ros::Publisher cloud_pub_;
diff --git a/ruvu_mcl/src/mcl.cpp b/ruvu_mcl/src/mcl.cpp
index 3be0b1c..a827a55 100644
--- a/ruvu_mcl/src/mcl.cpp
+++ b/ruvu_mcl/src/mcl.cpp
@@ -2,7 +2,6 @@
 
 #include "./mcl.hpp"
 
-#include <algorithm>
 #include <memory>
 
 #include "./adaptive/fixed.hpp"
@@ -17,7 +16,6 @@
 #include "./sensor_models/landmark_likelihood_field_model.hpp"
 #include "./sensor_models/likelihood_field_model.hpp"
 #include "ros/node_handle.h"
-#include "std_msgs/UInt32.h"
 #include "tf2/utils.h"
 
 constexpr auto name = "mcl";
@@ -37,12 +35,10 @@ std::unique_ptr<Laser> create_laser_model(
 }
 
 Mcl::Mcl(ros::NodeHandle nh, ros::NodeHandle private_nh)
-: cloud_pub_(nh, private_nh),
-  count_pub_(private_nh.advertise<std_msgs::UInt32>("count", 1)),
-  pose_pub_(private_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1)),
-  config_(),
+: config_(),
   rng_(std::make_unique<Rng>()),
   last_odom_pose_(),
+  last_filter_update_(),
   filter_(),
   model_(nullptr),
   map_(nullptr),
@@ -80,13 +76,8 @@ void Mcl::configure(const Config & config)
     throw std::logic_error("no adaptive algorithm configured");
 
   if (filter_.particles.empty()) {
-    auto p = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
-    p->header.stamp = ros::Time::now();
-    p->header.frame_id = config_.global_frame_id;
-    tf2::toMsg(config.initial_pose.getOrigin(), p->pose.pose.position);
-    p->pose.pose.orientation = tf2::toMsg(config.initial_pose.getRotation());
-    std::copy(config.initial_cov.begin(), config.initial_cov.end(), p->pose.covariance.begin());
-    initial_pose_cb(p);
+    PoseWithCovariance p{config.initial_pose, config.initial_cov};
+    initial_pose_cb(ros::Time::now(), p);
   }
 }
 
@@ -122,13 +113,7 @@ bool Mcl::scan_cb(const LaserData & scan, const tf2::Transform & odom_pose)
       resampler_->resample(&filter_, needed_particles);
   }
 
-  // Create output
-  auto ps = filter_.get_pose_with_covariance_stamped(scan.header.stamp, config_.global_frame_id);
-  publish_data(ps);
-  tf2::Transform pose;
-  tf2::fromMsg(ps.pose.pose, pose);
-  last_pose_.setData(pose);  // store last_pose_ for later use
-  last_pose_.stamp_ = scan.header.stamp;
+  last_filter_update_ = scan.header.stamp;
   return true;
 }
 
@@ -168,14 +153,7 @@ bool Mcl::landmark_cb(const LandmarkList & landmarks, const tf2::Transform & odo
       resampler_->resample(&filter_, needed_particles);
   }
 
-  // Create output
-  auto ps =
-    filter_.get_pose_with_covariance_stamped(landmarks.header.stamp, config_.global_frame_id);
-  publish_data(ps);
-  tf2::Transform pose;
-  tf2::fromMsg(ps.pose.pose, pose);
-  last_pose_.setData(pose);  // store last_pose_ for later use
-  last_pose_.stamp_ = landmarks.header.stamp;
+  last_filter_update_ = landmarks.header.stamp;
   return true;
 }
 
@@ -193,16 +171,17 @@ void Mcl::landmark_list_cb(const LandmarkList & landmarks)
   landmark_model_ = nullptr;
 }
 
-void Mcl::initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr & initial_pose)
+void Mcl::initial_pose_cb(const ros::Time & stamp, const PoseWithCovariance & initial_pose)
 {
-  const auto & p = initial_pose->pose;
+  const auto & p = initial_pose.pose;
+  const auto & covariance = initial_pose.covariance;
   ROS_INFO_NAMED(
-    name, "initial pose received %.3f %.3f %.3f, spawning %lu new particles", p.pose.position.x,
-    p.pose.position.y, tf2::getYaw(p.pose.orientation), config_.max_particles);
+    name, "initial pose received %.3f %.3f %.3f, spawning %lu new particles", p.getOrigin().x(),
+    p.getOrigin().y(), tf2::getYaw(p.getRotation()), config_.max_particles);
 
-  auto dx = rng_->normal_distribution(p.pose.position.x, p.covariance[0 * 6 + 0]);
-  auto dy = rng_->normal_distribution(p.pose.position.y, p.covariance[1 * 6 + 1]);
-  auto dt = rng_->normal_distribution(tf2::getYaw(p.pose.orientation), p.covariance[5 * 6 + 5]);
+  auto dx = rng_->normal_distribution(p.getOrigin().x(), covariance[0 * 6 + 0]);
+  auto dy = rng_->normal_distribution(p.getOrigin().y(), covariance[1 * 6 + 1]);
+  auto dt = rng_->normal_distribution(tf2::getYaw(p.getRotation()), covariance[5 * 6 + 5]);
 
   filter_.particles.clear();
   for (size_t i = 0; i < config_.max_particles; ++i) {
@@ -212,13 +191,7 @@ void Mcl::initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr
     filter_.particles.emplace_back(tf2::Transform{q, p}, 1. / config_.max_particles);
   }
 
-  auto ps =
-    filter_.get_pose_with_covariance_stamped(initial_pose->header.stamp, config_.global_frame_id);
-  publish_data(ps);
-  tf2::Transform pose;
-  tf2::fromMsg(ps.pose.pose, pose);
-  last_pose_.setData(pose);  // store last_pose_ for later use
-  last_pose_.stamp_ = initial_pose->header.stamp;
+  last_filter_update_ = stamp;
 }
 
 void Mcl::request_nomotion_update()
@@ -237,15 +210,15 @@ bool Mcl::odometry_update(
   if (!last_odom_pose_) {
     ROS_INFO_NAMED(name, "first odometry update, recording the odom pose");
     last_odom_pose_ = odom_pose;
-    last_pose_.stamp_ = header.stamp;
+    last_filter_update_ = header.stamp;
     // call should_process to record the frame_id
     should_process(tf2::Transform::getIdentity(), {measurement_type, header.frame_id});
     return true;
   }
 
-  if (header.stamp <= last_pose_.stamp_) {
+  if (header.stamp <= last_filter_update_) {
     ROS_DEBUG_STREAM_NAMED(
-      name, "skipping out-of-order measurement, " << header.stamp << " <= " << last_pose_.stamp_);
+      name, "skipping out-of-order measurement, " << header.stamp << " <= " << last_filter_update_);
     return false;
   }
 
@@ -290,15 +263,6 @@ bool Mcl::should_process(const tf2::Transform & diff, const MeasurementKey & mea
   }
 }
 
-void Mcl::publish_data(const geometry_msgs::PoseWithCovarianceStamped & ps)
-{
-  cloud_pub_.publish(ps.header, filter_);
-  std_msgs::UInt32 count;
-  count.data = filter_.particles.size();
-  count_pub_.publish(count);
-  pose_pub_.publish(ps);
-}
-
 std::ostream & operator<<(std::ostream & out, const Mcl::MeasurementType & measurement_type)
 {
   switch (measurement_type) {
diff --git a/ruvu_mcl/src/mcl.hpp b/ruvu_mcl/src/mcl.hpp
index 7303845..bbb813e 100644
--- a/ruvu_mcl/src/mcl.hpp
+++ b/ruvu_mcl/src/mcl.hpp
@@ -2,21 +2,21 @@
 
 #pragma once
 
+#include <boost/shared_ptr.hpp>
+#include <map>
 #include <memory>
 #include <string>
+#include <vector>
 
-#include "./cloud_publisher.hpp"
 #include "./config.hpp"
 #include "./particle_filter.hpp"
 #include "ros/message_forward.h"
-#include "ros/publisher.h"
-#include "tf2/transform_datatypes.h"
+#include "ros/time.h"
 
 // forward declare
 namespace ros
 {
 class NodeHandle;
-class Time;
 }  // namespace ros
 namespace tf2
 {
@@ -30,6 +30,10 @@ namespace nav_msgs
 {
 ROS_DECLARE_MESSAGE(OccupancyGrid)
 }
+namespace std_msgs
+{
+ROS_DECLARE_MESSAGE(Header)
+}
 
 namespace ruvu_mcl
 {
@@ -51,13 +55,17 @@ class Mcl
 
   void configure(const Config & config);
   const Config & config() const { return config_; }
-  const tf2::Transform & pose() const { return last_pose_; }
+  const PoseWithCovariance get_pose_with_covariance() const
+  {
+    return filter_.get_pose_with_covariance();
+  }
+  const std::vector<Particle> & particles() const { return filter_.particles; }
 
   bool scan_cb(const LaserData & scan, const tf2::Transform & odom_pose);
   bool landmark_cb(const LandmarkList & landmarks, const tf2::Transform & odom_pose);
   void map_cb(const nav_msgs::OccupancyGridConstPtr & map);
   void landmark_list_cb(const LandmarkList & landmarks);
-  void initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr & initial_pose);
+  void initial_pose_cb(const ros::Time & stamp, const PoseWithCovariance & initial_pose);
   void request_nomotion_update();
 
 private:
@@ -76,18 +84,12 @@ class Mcl
     const std_msgs::Header & header, const MeasurementType & measurement_type,
     tf2::Transform odom_pose);
   bool should_process(const tf2::Transform & diff, const MeasurementKey & measurment_key);
-  void publish_data(const geometry_msgs::PoseWithCovarianceStamped & ps);
-
-  // data output
-  CloudPublisher cloud_pub_;
-  ros::Publisher count_pub_;
-  ros::Publisher pose_pub_;
 
   // internals
   Config config_;
   std::shared_ptr<Rng> rng_;
   std::optional<tf2::Transform> last_odom_pose_;
-  tf2::Stamped<tf2::Transform> last_pose_;
+  ros::Time last_filter_update_;
   ParticleFilter filter_;
   std::unique_ptr<MotionModel> model_;
   nav_msgs::OccupancyGridConstPtr map_;
diff --git a/ruvu_mcl/src/mcl_ros.cpp b/ruvu_mcl/src/mcl_ros.cpp
index d38875b..8325f1d 100644
--- a/ruvu_mcl/src/mcl_ros.cpp
+++ b/ruvu_mcl/src/mcl_ros.cpp
@@ -2,12 +2,14 @@
 
 #include "./mcl_ros.hpp"
 
+#include <algorithm>
 #include <memory>
 
 #include "./sensor_models/landmark.hpp"
 #include "./sensor_models/laser.hpp"
 #include "ruvu_mcl_msgs/LandmarkList.h"
 #include "sensor_msgs/LaserScan.h"
+#include "std_msgs/UInt32.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #include "tf2_ros/buffer.h"
 
@@ -18,7 +20,13 @@ namespace ruvu_mcl
 MclRos::MclRos(
   ros::NodeHandle nh, ros::NodeHandle private_nh,
   const std::shared_ptr<const tf2_ros::Buffer> & buffer)
-: mcl_(nh, private_nh), buffer_(buffer), transform_br_(), last_tf_broadcast_()
+: mcl_(nh, private_nh),
+  buffer_(buffer),
+  transform_br_(),
+  last_tf_broadcast_(),
+  cloud_pub_(nh, private_nh),
+  count_pub_(private_nh.advertise<std_msgs::UInt32>("count", 1)),
+  pose_pub_(private_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1))
 {
 }
 
@@ -47,7 +55,9 @@ void MclRos::scan_cb(const sensor_msgs::LaserScanConstPtr & scan)
   auto updated = mcl_.scan_cb(data, odom_pose);
 
   if (updated) {
-    broadcast_tf(mcl_.pose(), odom_pose, data.header.stamp);
+    auto ps = mcl_.get_pose_with_covariance();
+    broadcast_tf(ps.pose, odom_pose, data.header.stamp);
+    publish_data(data.header.stamp, ps);
   } else if (!last_tf_broadcast_.header.frame_id.empty()) {  // Verify last_tf_broadcast_ is set
     broadcast_last_tf(data.header.stamp);
   }
@@ -74,7 +84,9 @@ void MclRos::landmark_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landmarks)
   auto updated = mcl_.landmark_cb(data, odom_pose);
 
   if (updated) {
-    broadcast_tf(mcl_.pose(), odom_pose, data.header.stamp);
+    auto ps = mcl_.get_pose_with_covariance();
+    broadcast_tf(ps.pose, odom_pose, data.header.stamp);
+    publish_data(data.header.stamp, ps);
   } else if (!last_tf_broadcast_.header.frame_id.empty()) {  // Verify last_tf_broadcast_ is set
     broadcast_last_tf(data.header.stamp);
   }
@@ -90,8 +102,25 @@ void MclRos::landmark_list_cb(const ruvu_mcl_msgs::LandmarkListConstPtr & landma
 
 void MclRos::initial_pose_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr & initial_pose)
 {
-  mcl_.initial_pose_cb(initial_pose);
+  if (initial_pose->header.frame_id != mcl_.config().global_frame_id) {
+    ROS_WARN_NAMED(name, "initial pose is only accepted in the global frame");
+  }
+
+  // convert PoseWithCovariance to MCL datatype
+  tf2::Transform p;
+  tf2::convert(initial_pose->pose.pose.position, p.getOrigin());
+  tf2::Quaternion q;
+  tf2::convert(initial_pose->pose.pose.orientation, q);
+  p.setRotation(q);
+  std::array<double, 36> covariance;
+  assert(initial_pose->pose.covariance.size() == covariance.size());
+  std::copy(
+    initial_pose->pose.covariance.begin(), initial_pose->pose.covariance.end(), covariance.begin());
+  PoseWithCovariance pose_with_covariance{p, covariance};
+
+  mcl_.initial_pose_cb(initial_pose->header.stamp, pose_with_covariance);
   mcl_.request_nomotion_update();
+  publish_data(initial_pose->header.stamp, pose_with_covariance);
 }
 
 tf2::Transform MclRos::get_odom_pose(const ros::Time & time) const
@@ -131,4 +160,26 @@ void MclRos::broadcast_last_tf(const ros::Time & stamp)
   transform_br_.sendTransform(msg);
   last_tf_broadcast_.header.stamp = stamp;
 }
+
+void MclRos::publish_data(const ros::Time & stamp, const PoseWithCovariance & pose_with_covariance)
+{
+  std_msgs::Header header;
+  header.stamp = stamp;
+  header.frame_id = mcl_.config().global_frame_id;
+  cloud_pub_.publish(header, mcl_.particles());
+
+  std_msgs::UInt32 count;
+  count.data = mcl_.particles().size();
+  count_pub_.publish(count);
+
+  geometry_msgs::PoseWithCovarianceStamped ps;
+  ps.header = header;
+  tf2::toMsg(pose_with_covariance.pose.getOrigin(), ps.pose.pose.position);
+  tf2::convert(pose_with_covariance.pose.getRotation(), ps.pose.pose.orientation);
+  assert(pose_with_covariance.covariance.size() == ps.pose.covariance.size());
+  std::copy(
+    pose_with_covariance.covariance.begin(), pose_with_covariance.covariance.end(),
+    ps.pose.covariance.begin());
+  pose_pub_.publish(ps);
+}
 }  // namespace ruvu_mcl
diff --git a/ruvu_mcl/src/mcl_ros.hpp b/ruvu_mcl/src/mcl_ros.hpp
index a98dfa0..006cade 100644
--- a/ruvu_mcl/src/mcl_ros.hpp
+++ b/ruvu_mcl/src/mcl_ros.hpp
@@ -4,6 +4,7 @@
 
 #include <memory>
 
+#include "./cloud_publisher.hpp"
 #include "./mcl.hpp"
 #include "ros/message_forward.h"
 #include "tf2_ros/transform_broadcaster.h"
@@ -58,10 +59,14 @@ class MclRos
   void broadcast_tf(
     const tf2::Transform & pose, const tf2::Transform & odom_pose, const ros::Time & stamp);
   void broadcast_last_tf(const ros::Time & stamp);
+  void publish_data(const ros::Time & stamp, const PoseWithCovariance & pose_with_covariance);
 
   Mcl mcl_;
   std::shared_ptr<const tf2_ros::Buffer> buffer_;
   tf2_ros::TransformBroadcaster transform_br_;
   geometry_msgs::TransformStamped last_tf_broadcast_;
+  CloudPublisher cloud_pub_;
+  ros::Publisher count_pub_;
+  ros::Publisher pose_pub_;
 };
 }  // namespace ruvu_mcl
diff --git a/ruvu_mcl/src/particle_filter.cpp b/ruvu_mcl/src/particle_filter.cpp
index 2d0fdcd..c35751d 100644
--- a/ruvu_mcl/src/particle_filter.cpp
+++ b/ruvu_mcl/src/particle_filter.cpp
@@ -2,11 +2,7 @@
 
 #include "./particle_filter.hpp"
 
-#include <algorithm>
-#include <string>
-
 #include "ros/console.h"
-#include "tf2/utils.h"
 
 constexpr auto name = "particle_filter";
 
@@ -52,14 +48,10 @@ double ParticleFilter::calc_effective_sample_size() const
   return 1. / sq_weight;
 }
 
-geometry_msgs::PoseWithCovarianceStamped ParticleFilter::get_pose_with_covariance_stamped(
-  const ros::Time & stamp, const std::string & frame_id) const
+PoseWithCovariance ParticleFilter::get_pose_with_covariance() const
 {
   std::array<double, 36> cov = {0};
   double mean[4] = {0};
-  geometry_msgs::PoseWithCovarianceStamped avg_pose;
-  avg_pose.header.stamp = stamp;
-  avg_pose.header.frame_id = frame_id;
 
   for (const auto & particle : particles) {
     // the four components of mean are: average x position, average y position,
@@ -87,15 +79,14 @@ geometry_msgs::PoseWithCovarianceStamped ParticleFilter::get_pose_with_covarianc
 
   // Covariance in angular component
   cov[35] = -2 * log(sqrt(mean[2] * mean[2] + mean[3] * mean[3]));
-  static_assert(cov.size() == avg_pose.pose.covariance.size());
-  std::copy(cov.begin(), cov.end(), avg_pose.pose.covariance.begin());
 
   // Mean pose
-  avg_pose.pose.pose.position.x = mean[0];
-  avg_pose.pose.pose.position.y = mean[1];
+  tf2::Transform avg_pose;
+  avg_pose.getOrigin()[0] = mean[0];
+  avg_pose.getOrigin()[1] = mean[1];
   tf2::Quaternion avg_q;
   avg_q.setRPY(0, 0, atan2(mean[3], mean[2]));
-  tf2::convert(avg_q, avg_pose.pose.pose.orientation);
-  return avg_pose;
+  avg_pose.setRotation(avg_q);
+  return PoseWithCovariance{avg_pose, cov};
 }
 }  // namespace ruvu_mcl
diff --git a/ruvu_mcl/src/particle_filter.hpp b/ruvu_mcl/src/particle_filter.hpp
index 83fd6c5..b811d5c 100644
--- a/ruvu_mcl/src/particle_filter.hpp
+++ b/ruvu_mcl/src/particle_filter.hpp
@@ -5,11 +5,20 @@
 #include <array>
 #include <vector>
 
-#include "geometry_msgs/PoseWithCovarianceStamped.h"
 #include "tf2/LinearMath/Transform.h"
 
 namespace ruvu_mcl
 {
+struct PoseWithCovariance
+{
+  PoseWithCovariance(const tf2::Transform & pose, const std::array<double, 36> & covariance)
+  : pose(pose), covariance(covariance)
+  {
+  }
+  tf2::Transform pose;
+  std::array<double, 36> covariance;
+};
+
 class Particle
 {
 public:
@@ -27,7 +36,6 @@ class ParticleFilter
   void normalize_weights();
   void normalize_weights(double total_weight);
   double calc_effective_sample_size() const;
-  geometry_msgs::PoseWithCovarianceStamped get_pose_with_covariance_stamped(
-    const ros::Time & stamp, const std::string & frame_id) const;
+  PoseWithCovariance get_pose_with_covariance() const;
 };
 }  // namespace ruvu_mcl
diff --git a/ruvu_mcl/test/test_particle_filter.cpp b/ruvu_mcl/test/test_particle_filter.cpp
index 0af2b62..6266aec 100644
--- a/ruvu_mcl/test/test_particle_filter.cpp
+++ b/ruvu_mcl/test/test_particle_filter.cpp
@@ -53,7 +53,7 @@ TEST_F(ParticleFilterTest, EffectiveSampleSize)
 TEST_F(ParticleFilterTest, Covariance)
 {
   pf.normalize_weights();
-  auto cov = pf.get_pose_with_covariance_stamped(ros::Time(), "map").pose.covariance;
+  auto cov = pf.get_pose_with_covariance().covariance;
 
   std::array<double, 4> sum_of_squares = {0};
   std::array<double, 4> sum = {0};
@@ -117,7 +117,7 @@ TEST_F(ParticleFilterCovarianceTest, Covariance)
   SetUp(0, 0.5);
   constexpr double eps = 5e-2;
 
-  auto cov = pf.get_pose_with_covariance_stamped(ros::Time(), "map").pose.covariance;
+  auto cov = pf.get_pose_with_covariance().covariance;
   EXPECT_NEAR(cov[0], pow(0.5, 2), eps);
   EXPECT_NEAR(cov[1], 0, eps);
   EXPECT_NEAR(cov[6], 0, eps);
@@ -129,7 +129,7 @@ TEST_F(ParticleFilterCovarianceTest, CovarianceWithOffset)
   SetUp(3, 0.5);
   constexpr double eps = 5e-2;
 
-  auto cov = pf.get_pose_with_covariance_stamped(ros::Time(), "map").pose.covariance;
+  auto cov = pf.get_pose_with_covariance().covariance;
   EXPECT_NEAR(cov[0], pow(0.5, 2), eps);
   EXPECT_NEAR(cov[1], 0, eps);
   EXPECT_NEAR(cov[6], 0, eps);
@@ -156,7 +156,7 @@ TEST_F(ParticleFilterCovarianceTest, CovarianceTransformed)
   // https://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix/
   auto expected = r * s * s * r.transpose();
 
-  auto cov = pf.get_pose_with_covariance_stamped(ros::Time(), "map").pose.covariance;
+  auto cov = pf.get_pose_with_covariance().covariance;
   EXPECT_NEAR(cov[0], expected[0][0], eps);
   EXPECT_NEAR(cov[1], expected[0][1], eps);
   EXPECT_NEAR(cov[6], expected[1][0], eps);