diff --git a/src/lib/BasicLaserMapping.cpp b/src/lib/BasicLaserMapping.cpp
index 349f0ba8..c0e98d2e 100644
--- a/src/lib/BasicLaserMapping.cpp
+++ b/src/lib/BasicLaserMapping.cpp
@@ -848,6 +848,9 @@ void BasicLaserMapping::optimizeTransformTobeMapped()
       matAtB = matAt * matB;
       matX = matAtA.colPivHouseholderQr().solve(matAtB);
 
+      // NOTE: Handling with the degeneracy problem according to the paper
+      // J. Zhang, M. Kaess and S. Singh, "On degeneracy of optimization-based state estimation problems,"
+      // 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016
       if (iterCount == 0)
       {
          Eigen::Matrix<float, 1, 6> matE;
@@ -877,7 +880,7 @@ void BasicLaserMapping::optimizeTransformTobeMapped()
                break;
             }
          }
-         matP = matV.inverse() * matV2;
+         matP = matV2 * matV.inverse();
       }
 
       if (isDegenerate)
@@ -908,4 +911,4 @@ void BasicLaserMapping::optimizeTransformTobeMapped()
 }
 
 
-} // end namespace loam
\ No newline at end of file
+} // end namespace loam
diff --git a/src/lib/BasicLaserOdometry.cpp b/src/lib/BasicLaserOdometry.cpp
index 1506f1e5..d430b884 100644
--- a/src/lib/BasicLaserOdometry.cpp
+++ b/src/lib/BasicLaserOdometry.cpp
@@ -558,6 +558,9 @@ void BasicLaserOdometry::process()
 
          matX = matAtA.colPivHouseholderQr().solve(matAtB);
 
+         // NOTE: Handling with the degeneracy problem according to the paper
+         // J. Zhang, M. Kaess and S. Singh, "On degeneracy of optimization-based state estimation problems,"
+         // 2016 IEEE International Conference on Robotics and Automation (ICRA), Stockholm, 2016
          if (iterCount == 0)
          {
             Eigen::Matrix<float, 1, 6> matE;
@@ -587,7 +590,7 @@ void BasicLaserOdometry::process()
                   break;
                }
             }
-            matP = matV.inverse() * matV2;
+            matP = matV2 * matV.inverse();
          }
 
          if (isDegenerate)