diff --git a/src/Base/Matrix.cpp b/src/Base/Matrix.cpp
index 7f0ac424fe..c6a870eddf 100644
--- a/src/Base/Matrix.cpp
+++ b/src/Base/Matrix.cpp
@@ -26,7 +26,6 @@
 # include <cstring>
 # include <sstream>
 #endif
-# include <array>
 
 #include "Matrix.h"
 #include "Converter.h"
@@ -90,16 +89,23 @@ void Matrix4D::setToUnity ()
 }
 
 bool Matrix4D::isUnity() const
+{
+    return isUnity(0.0);
+}
+
+bool Matrix4D::isUnity(double tol) const
 {
     for (int i = 0; i < 4; i++) {
         for (int j = 0; j < 4; j++) {
             if (i == j) {
-                if (dMtrx4D[i][j] != 1.0)
+                if (fabs(dMtrx4D[i][j] - 1.0) > tol) {
                     return false;
+                }
             }
             else {
-                if (dMtrx4D[i][j] != 0.0)
+                if (fabs(dMtrx4D[i][j]) > tol) {
                     return false;
+                }
             }
         }
     }
@@ -899,7 +905,8 @@ ScaleType Matrix4D::hasScale(double tol) const
     return ScaleType::NoScaling;
 }
 
-std::array<Matrix4D, 4> Matrix4D::decompose() const {
+std::array<Matrix4D, 4> Matrix4D::decompose() const
+{
     // decompose the matrix to shear, scale, rotation and move
     // so that matrix = move * rotation * scale * shear
     // return an array of matrices
diff --git a/src/Base/Matrix.h b/src/Base/Matrix.h
index 163dee1a19..d0c2addc6f 100644
--- a/src/Base/Matrix.h
+++ b/src/Base/Matrix.h
@@ -149,6 +149,8 @@ public:
     void setToUnity();
     /// Checks if this is the unit matrix
     bool isUnity() const;
+    /// Checks if this is the unit matrix
+    bool isUnity(double tol) const;
     /// Makes a null matrix
     void nullify();
     /// Checks if this is the null matrix
diff --git a/src/Base/MatrixPy.xml b/src/Base/MatrixPy.xml
index c2dca83498..cd70049b76 100644
--- a/src/Base/MatrixPy.xml
+++ b/src/Base/MatrixPy.xml
@@ -125,9 +125,9 @@ Check if this is the null matrix.</UserDocu>
 Make this matrix to unity (4D identity matrix).</UserDocu>
       </Documentation>
     </Methode>
-    <Methode Name="isUnity" Const="true" NoArgs="true">
+    <Methode Name="isUnity" Const="true">
       <Documentation>
-        <UserDocu>isUnity() -> bool
+        <UserDocu>isUnity([tol=0.0]) -> bool
 
 Check if this is the unit matrix (4D identity matrix).</UserDocu>
       </Documentation>
diff --git a/src/Base/MatrixPyImp.cpp b/src/Base/MatrixPyImp.cpp
index 8bacece2a8..5115782263 100644
--- a/src/Base/MatrixPyImp.cpp
+++ b/src/Base/MatrixPyImp.cpp
@@ -22,7 +22,6 @@
 
 
 #include "PreCompiled.h"
-//#include <array>
 
 // inclusion of the generated files (generated out of MatrixPy.xml)
 #include "RotationPy.h"
@@ -352,6 +351,7 @@ PyObject* MatrixPy::hasScale(PyObject * args)
     Py::Module mod("FreeCAD");
     return Py::new_reference_to(mod.callMemberFunction("ScaleType", Py::TupleN(Py::Int(static_cast<int>(type)))));
 }
+
 PyObject* MatrixPy::decompose(PyObject * args)
 {
     if (!PyArg_ParseTuple(args, ""))
@@ -392,10 +392,13 @@ PyObject* MatrixPy::unity()
     PY_CATCH;
 }
 
-PyObject* MatrixPy::isUnity()
+PyObject* MatrixPy::isUnity(PyObject * args)
 {
+    double tol = 0.0;
+    if (!PyArg_ParseTuple(args, "|d", &tol))
+        return nullptr;
     PY_TRY {
-        bool ok = getMatrixPtr()->isUnity();
+        bool ok = getMatrixPtr()->isUnity(tol);
         return Py::new_reference_to(Py::Boolean(ok));
     }
     PY_CATCH;