diff --git a/src/core/include/pw/core/matrixbase.hpp b/src/core/include/pw/core/matrixbase.hpp
index c067546..e0d1a26 100644
--- a/src/core/include/pw/core/matrixbase.hpp
+++ b/src/core/include/pw/core/matrixbase.hpp
@@ -95,8 +95,8 @@ public:
     //! set identity
     inline matrixbase& set_identity() {
         for (unsigned int r = 0;r < rows(); r++)
-            for (unsigned int c = r; c < cols(); c++)
-                this->at(r,c) = (c == r) ? 1 : 0;
+            for (unsigned int c = 0; c < cols(); c++)
+                this->at(r,c) = (c == r) ? T(1) : T(0);
 		return *this;
 	}