diff --git a/include/mcl_3dl/chunked_kdtree.h b/include/mcl_3dl/chunked_kdtree.h
index eb27dac4..92735bd8 100644
--- a/include/mcl_3dl/chunked_kdtree.h
+++ b/include/mcl_3dl/chunked_kdtree.h
@@ -71,7 +71,7 @@ class ChunkedKdtree
     {
       return (id.x_) ^ (id.y_ << 11) ^ (id.z_ << 22);
     }
-    ChunkId(const int& x, const int& y, const int& z)
+    ChunkId(const int x, const int y, const int z)
       : x_(x)
       , y_(y)
       , z_(z)
@@ -228,7 +228,7 @@ class ChunkedKdtree
 
     return ret;
   }
-  typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr getChunkKdtree(const POINT_TYPE p)
+  typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr getChunkKdtree(const POINT_TYPE& p)
   {
     return getChunkKdtree(getChunkId(p));
   }
@@ -238,7 +238,7 @@ class ChunkedKdtree
       return typename pcl::KdTreeFLANN<POINT_TYPE>::Ptr();
     return chunks_[c].kdtree_;
   }
-  typename pcl::PointCloud<POINT_TYPE>::Ptr getChunkCloud(const POINT_TYPE p)
+  typename pcl::PointCloud<POINT_TYPE>::Ptr getChunkCloud(const POINT_TYPE& p)
   {
     return getChunkCloud(getChunkId(p));
   }
@@ -248,7 +248,7 @@ class ChunkedKdtree
       return typename pcl::PointCloud<POINT_TYPE>::Ptr();
     return chunks_[c].cloud_;
   }
-  ChunkId getChunkId(const POINT_TYPE p) const
+  ChunkId getChunkId(const POINT_TYPE& p) const
   {
     return ChunkId(static_cast<int>(floor(p.x * pos_to_chunk_)),
                    static_cast<int>(floor(p.y * pos_to_chunk_)),
diff --git a/include/mcl_3dl/pf.h b/include/mcl_3dl/pf.h
index 0584cbd6..c4adef28 100644
--- a/include/mcl_3dl/pf.h
+++ b/include/mcl_3dl/pf.h
@@ -186,7 +186,7 @@ class ParticleFilter
     const FLT_TYPE prob = 1.0 / particles_.size();
     for (size_t i = 0; i < particles_.size(); ++i)
     {
-      auto &p = particles_[i];
+      auto& p = particles_[i];
       const FLT_TYPE pscan = std::nextafter(pstep * (i + 1), static_cast<FLT_TYPE>(0.0));
       it = std::lower_bound(it, particles_dup_.end(), Particle<T, FLT_TYPE>(pscan));
       p.probability_ = prob;
@@ -274,14 +274,16 @@ class ParticleFilter
     }
     return mean.getMean();
   }
-  std::vector<T> covariance(const FLT_TYPE pass_ratio = 1.0)
+  std::vector<T> covariance(
+      const FLT_TYPE pass_ratio = 1.0,
+      const FLT_TYPE random_sample_ratio = 1.0)
   {
     T e = expectation(pass_ratio);
     FLT_TYPE p_sum = 0;
-    size_t p_num = 0;
     std::vector<T> cov;
     cov.resize(e.size());
 
+    size_t p_num = 0;
     for (auto& p : particles_)
     {
       p_num++;
@@ -289,9 +291,27 @@ class ParticleFilter
       if (p_sum > pass_ratio)
         break;
     }
-    p_sum = 0;
-    for (auto& p : particles_)
+
+    std::vector<size_t> indices(p_num);
+    std::iota(indices.begin(), indices.end(), 0);
+    if (random_sample_ratio < 1.0)
     {
+      std::shuffle(indices.begin(), indices.end(), engine_);
+
+      const size_t sample_num =
+          std::min(
+              p_num,
+              std::max(
+                  size_t(0),
+                  static_cast<size_t>(p_num * random_sample_ratio)));
+      indices.resize(sample_num);
+    }
+
+    p_sum = 0.0;
+    for (size_t i : indices)
+    {
+      auto& p = particles_[i];
+      p_sum += p.probability_;
       for (size_t j = 0; j < ie_.size(); j++)
       {
         for (size_t k = j; k < ie_.size(); k++)
@@ -299,17 +319,12 @@ class ParticleFilter
           cov[k][j] = cov[j][k] += p.state_.covElement(e, j, k) * p.probability_;
         }
       }
-
-      p_sum += p.probability_;
-      if (p_sum > pass_ratio)
-        break;
     }
     for (size_t j = 0; j < ie_.size(); j++)
     {
-      for (size_t k = j; k < ie_.size(); k++)
+      for (size_t k = 0; k < ie_.size(); k++)
       {
         cov[k][j] /= p_sum;
-        cov[j][k] /= p_sum;
       }
     }
 
diff --git a/src/mcl_3dl.cpp b/src/mcl_3dl.cpp
index d0498b4d..f85338a5 100644
--- a/src/mcl_3dl.cpp
+++ b/src/mcl_3dl.cpp
@@ -655,7 +655,12 @@ class MCL3dlNode
     if (publish_tf_)
       tfb_.sendTransform(transforms);
 
-    auto cov = pf_->covariance();
+    // Calculate covariance from sampled particles to reduce calculation cost on global localization.
+    // Use the number of original particles or at least 10% of full particles.
+    auto cov = pf_->covariance(
+        1.0,
+        std::max(
+            0.1f, static_cast<float>(params_.num_particles_) / pf_->getParticleSize()));
 
     geometry_msgs::PoseWithCovarianceStamped pose;
     pose.header.stamp = msg->header.stamp;
@@ -1047,7 +1052,7 @@ class MCL3dlNode
       }
     }
     response.success = true;
-    response.message = std::to_string(points->size()) + " particles";
+    response.message = std::to_string(pf_->getParticleSize()) + " particles";
     return true;
   }
 
diff --git a/test/src/test_pf.cpp b/test/src/test_pf.cpp
index 6726121c..35db93b9 100644
--- a/test/src/test_pf.cpp
+++ b/test/src/test_pf.cpp
@@ -137,6 +137,9 @@ TEST(Pf, BayesianEstimation)
       pf.resample(State(0.0));
       ASSERT_NEAR(avg, pf.expectation()[0], abs_error);
       ASSERT_NEAR(var, pf.covariance()[0][0], abs_error);
+
+      // 50% Random sampled covaliance calculation
+      ASSERT_NEAR(var, pf.covariance(1.0, 0.5)[0][0], abs_error * 2);
     }
   }
 }
@@ -202,17 +205,17 @@ TEST(Pf, ResampleFlatLikelihood)
 TEST(Pf, ResampleFirstParticle)
 {
   const std::vector<float> probs =
-  {
-    0.0001f, 0.2f, 0.2f, 0.2f, 0.3999f
-  };
+      {
+        0.0001f, 0.2f, 0.2f, 0.2f, 0.3999f
+      };
   const std::vector<float> states =
-  {
-    0.0f, 1.0f, 2.0f, 3.0f, 4.0f
-  };
+      {
+        0.0f, 1.0f, 2.0f, 3.0f, 4.0f
+      };
   const std::vector<float> expected_resampled_states =
-  {
-    1.0f, 2.0f, 3.0f, 4.0f, 4.0f
-  };
+      {
+        1.0f, 2.0f, 3.0f, 4.0f, 4.0f
+      };
   const size_t particle_num = probs.size();
 
   mcl_3dl::pf::ParticleFilter<State, float> pf(particle_num);