diff --git a/src/icp.cpp b/src/icp.cpp
index 617a09b4e43819c77d86c8d65608bf1fde4e598e..cf8379812f044aeeeb4a1e8e9001949d1ee9001f 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -94,8 +94,8 @@ icpOutput ICP::matchPC(LaserScan &_last_ls, LaserScan &_origin_ls, LaserScanPara
     result.nvalid = csm_output.nvalid;
     result.error  = csm_output.error;
 
-    std::cout << "ICP: add covariance!" << '\n';
-    // result.res_covar = Eigen::Zeros();
+
+    result.res_covar = Eigen::Matrix3s::Identity()*0.1;
 
     // std::cout << "Number of valid correspondences: " << csm_output.nvalid << '\n';
     // std::cout << "Number of iterations: " << csm_output.iterations << '\n';