diff --git a/CMakeLists.txt b/CMakeLists.txt
index bfbaef599d..10040a237c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,7 +49,7 @@ enable_testing()
 
 include_directories(matrix)
 
-file(GLOB_RECURSE COV_SRCS matrix/*.hpp matrix/*.cpp test/*.hpp test/*.cpp)
+file(GLOB_RECURSE COV_SRCS matrix/*.hpp matrix/*.cpp)
 
 # Setup the coveralls target and tell it to gather
 # coverage data for all the lib sources.
diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 6c37e63bcd..dc72f85921 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -151,7 +151,8 @@ public:
         self = self - other;
     }
 
-    void operator*=(const Matrix<Type, M, N> &other)
+    template<size_t P>
+    void operator*=(const Matrix<Type, N, P> &other)
     {
         Matrix<Type, M, N> &self = *this;
         self = self * other;
@@ -247,42 +248,6 @@ public:
         return transpose();
     }
 
-    Matrix<Type, M, M> expm(float dt, size_t n) const
-    {
-        Matrix<float, M, M> res;
-        res.setIdentity();
-        Matrix<float, M, M> A_pow = *this;
-        float k_fact = 1;
-        size_t k = 1;
-
-        while (k < n) {
-            res += A_pow * (float(pow(dt, k)) / k_fact);
-
-            if (k == n) {
-                break;
-            }
-
-            A_pow *= A_pow;
-            k_fact *= k;
-            k++;
-        }
-
-        return res;
-    }
-
-    Vector<Type, M> diagonal() const
-    {
-        Vector<Type, M> res;
-        // force square for now
-        const Matrix<Type, M, M> &self = *this;
-
-        for (size_t i = 0; i < M; i++) {
-            res(i) = self(i, i);
-        }
-
-        return res;
-    }
-
     void setZero()
     {
         memset(_data, 0, sizeof(_data));
@@ -328,133 +293,8 @@ public:
         }
     }
 
-    /**
-     * inverse based on LU factorization with partial pivotting
-     */
-    Matrix <Type, M, M> inverse() const
-    {
-        Matrix<Type, M, M> L;
-        L.setIdentity();
-        const Matrix<Type, M, M> &A = (*this);
-        Matrix<Type, M, M> U = A;
-        Matrix<Type, M, M> P;
-        P.setIdentity();
-
-        //printf("A:\n"); A.print();
-
-        // for all diagonal elements
-        for (size_t n = 0; n < N; n++) {
-
-            // if diagonal is zero, swap with row below
-            if (fabsf(U(n, n)) < 1e-8f) {
-                //printf("trying pivot for row %d\n",n);
-                for (size_t i = 0; i < N; i++) {
-                    if (i == n) {
-                        continue;
-                    }
-
-                    //printf("\ttrying row %d\n",i);
-                    if (fabsf(U(i, n)) > 1e-8f) {
-                        //printf("swapped %d\n",i);
-                        U.swapRows(i, n);
-                        P.swapRows(i, n);
-                    }
-                }
-            }
-
-#ifdef MATRIX_ASSERT
-            //printf("A:\n"); A.print();
-            //printf("U:\n"); U.print();
-            //printf("P:\n"); P.print();
-            //fflush(stdout);
-            ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-            // failsafe, return zero matrix
-            if (fabsf(U(n, n)) < 1e-8f) {
-                Matrix<Type, M, M> zero;
-                zero.setZero();
-                return zero;
-            }
-
-            // for all rows below diagonal
-            for (size_t i = (n + 1); i < N; i++) {
-                L(i, n) = U(i, n) / U(n, n);
-
-                // add i-th row and n-th row
-                // multiplied by: -a(i,n)/a(n,n)
-                for (size_t k = n; k < N; k++) {
-                    U(i, k) -= L(i, n) * U(n, k);
-                }
-            }
-        }
-
-        //printf("L:\n"); L.print();
-        //printf("U:\n"); U.print();
-
-        // solve LY=P*I for Y by forward subst
-        Matrix<Type, M, M> Y = P;
-
-        // for all columns of Y
-        for (size_t c = 0; c < N; c++) {
-            // for all rows of L
-            for (size_t i = 0; i < N; i++) {
-                // for all columns of L
-                for (size_t j = 0; j < i; j++) {
-                    // for all existing y
-                    // subtract the component they
-                    // contribute to the solution
-                    Y(i, c) -= L(i, j) * Y(j, c);
-                }
-
-                // divide by the factor
-                // on current
-                // term to be solved
-                // Y(i,c) /= L(i,i);
-                // but L(i,i) = 1.0
-            }
-        }
-
-        //printf("Y:\n"); Y.print();
-
-        // solve Ux=y for x by back subst
-        Matrix<Type, M, M> X = Y;
-
-        // for all columns of X
-        for (size_t c = 0; c < N; c++) {
-            // for all rows of U
-            for (size_t k = 0; k < N; k++) {
-                // have to go in reverse order
-                size_t i = N - 1 - k;
-
-                // for all columns of U
-                for (size_t j = i + 1; j < N; j++) {
-                    // for all existing x
-                    // subtract the component they
-                    // contribute to the solution
-                    X(i, c) -= U(i, j) * X(j, c);
-                }
-
-                // divide by the factor
-                // on current
-                // term to be solved
-                X(i, c) /= U(i, i);
-            }
-        }
-
-        //printf("X:\n"); X.print();
-        return X;
-    }
-
-    // inverse alias
-    inline Matrix<Type, N, M> I() const
-    {
-        return inverse();
-    }
-
 };
 
-
 typedef Matrix<float, 3, 3> Matrix3f;
 
 }; // namespace matrix
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
new file mode 100644
index 0000000000..059a2d5cc5
--- /dev/null
+++ b/matrix/SquareMatrix.hpp
@@ -0,0 +1,207 @@
+/**
+ * @file SquareMatrix.hpp
+ *
+ * A square matrix
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include <stdio.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "Matrix.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
+template<typename Type, size_t  M>
+class SquareMatrix : public Matrix<Type, M, M>
+{
+public:
+    SquareMatrix() :
+        Matrix<Type, M, M>()
+    {
+    }
+
+    SquareMatrix(const Type *data) :
+        Matrix<Type, M, M>(data)
+    {
+    }
+
+    SquareMatrix(const Matrix<float, M, M> &other) :
+        Matrix<Type, M, M>(other)
+    {
+    }
+
+    /**
+     * inverse based on LU factorization with partial pivotting
+     */
+    SquareMatrix <Type, M> inverse() const
+    {
+        SquareMatrix<Type, M> L;
+        L.setIdentity();
+        const SquareMatrix<Type, M> &A = (*this);
+        SquareMatrix<Type, M> U = A;
+        SquareMatrix<Type, M> P;
+        P.setIdentity();
+
+        //printf("A:\n"); A.print();
+
+        // for all diagonal elements
+        for (size_t n = 0; n < M; n++) {
+
+            // if diagonal is zero, swap with row below
+            if (fabsf(U(n, n)) < 1e-8f) {
+                //printf("trying pivot for row %d\n",n);
+                for (size_t i = 0; i < M; i++) {
+                    if (i == n) {
+                        continue;
+                    }
+
+                    //printf("\ttrying row %d\n",i);
+                    if (fabsf(U(i, n)) > 1e-8f) {
+                        //printf("swapped %d\n",i);
+                        U.swapRows(i, n);
+                        P.swapRows(i, n);
+                    }
+                }
+            }
+
+#ifdef MATRIX_ASSERT
+            //printf("A:\n"); A.print();
+            //printf("U:\n"); U.print();
+            //printf("P:\n"); P.print();
+            //fflush(stdout);
+            ASSERT(fabsf(U(n, n)) > 1e-8f);
+#endif
+
+            // failsafe, return zero matrix
+            if (fabsf(U(n, n)) < 1e-8f) {
+                SquareMatrix<Type, M> zero;
+                zero.setZero();
+                return zero;
+            }
+
+            // for all rows below diagonal
+            for (size_t i = (n + 1); i < M; i++) {
+                L(i, n) = U(i, n) / U(n, n);
+
+                // add i-th row and n-th row
+                // multiplied by: -a(i,n)/a(n,n)
+                for (size_t k = n; k < M; k++) {
+                    U(i, k) -= L(i, n) * U(n, k);
+                }
+            }
+        }
+
+        //printf("L:\n"); L.print();
+        //printf("U:\n"); U.print();
+
+        // solve LY=P*I for Y by forward subst
+        SquareMatrix<Type, M> Y = P;
+
+        // for all columns of Y
+        for (size_t c = 0; c < M; c++) {
+            // for all rows of L
+            for (size_t i = 0; i < M; i++) {
+                // for all columns of L
+                for (size_t j = 0; j < i; j++) {
+                    // for all existing y
+                    // subtract the component they
+                    // contribute to the solution
+                    Y(i, c) -= L(i, j) * Y(j, c);
+                }
+
+                // divide by the factor
+                // on current
+                // term to be solved
+                // Y(i,c) /= L(i,i);
+                // but L(i,i) = 1.0
+            }
+        }
+
+        //printf("Y:\n"); Y.print();
+
+        // solve Ux=y for x by back subst
+        SquareMatrix<Type, M> X = Y;
+
+        // for all columns of X
+        for (size_t c = 0; c < M; c++) {
+            // for all rows of U
+            for (size_t k = 0; k < M; k++) {
+                // have to go in reverse order
+                size_t i = M - 1 - k;
+
+                // for all columns of U
+                for (size_t j = i + 1; j < M; j++) {
+                    // for all existing x
+                    // subtract the component they
+                    // contribute to the solution
+                    X(i, c) -= U(i, j) * X(j, c);
+                }
+
+                // divide by the factor
+                // on current
+                // term to be solved
+                X(i, c) /= U(i, i);
+            }
+        }
+
+        //printf("X:\n"); X.print();
+        return X;
+    }
+
+    // inverse alias
+    inline SquareMatrix<Type, M> I() const
+    {
+        return inverse();
+    }
+
+    Vector<Type, M> diagonal() const
+    {
+        Vector<Type, M> res;
+        const SquareMatrix<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res(i) = self(i, i);
+        }
+        return res;
+    }
+
+    SquareMatrix<Type, M> expm(float dt, size_t n) const
+    {
+        SquareMatrix<float, M> res;
+        res.setIdentity();
+        SquareMatrix<float, M> A_pow = *this;
+        float k_fact = 1;
+        size_t k = 1;
+
+        while (k < n) {
+            res += A_pow * (float(pow(dt, k)) / k_fact);
+
+            if (k == n) {
+                break;
+            }
+
+            A_pow *= A_pow;
+            k_fact *= k;
+            k++;
+        }
+
+        return res;
+    }
+};
+
+typedef SquareMatrix<float, 3> SquareMatrix3f;
+
+}; // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f144297156..4c37fe51c9 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,9 +1,3 @@
-add_library(instantiation
-	instantiation.cpp
-	)
-
-link_libraries(instantiation)
-
 set(tests
 	setIdentity
 	inverse
diff --git a/test/instantiation.cpp b/test/instantiation.cpp
deleted file mode 100644
index 9f425d0713..0000000000
--- a/test/instantiation.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-// dummy code to instantiate all templates for coverage
-
-#include <Matrix.hpp>
-#include <Vector.hpp>
-#include <Vector3.hpp>
-#include <Euler.hpp>
-#include <Scalar.hpp>
-#include <Quaternion.hpp>
-
-namespace matrix {
-
-template class Vector<float, 2>;
-template class Vector<float, 3>;
-template class Vector<float, 4>;
-template class Euler<float>;
-template class Scalar<float>;
-template class Matrix<float, 3, 3>;
-template class Matrix<float, 1, 1>;
-template class Quaternion<float>;
-
-};
diff --git a/test/inverse.cpp b/test/inverse.cpp
index 9fb23fdefa..21a2309659 100644
--- a/test/inverse.cpp
+++ b/test/inverse.cpp
@@ -1,27 +1,31 @@
-#include "Matrix.hpp"
+#include "SquareMatrix.hpp"
 #include <assert.h>
 #include <stdio.h>
 
 using namespace matrix;
 
+static const size_t n_large = 50;
+
+template class SquareMatrix<float, 3>;
+template class SquareMatrix<float, n_large>;
+
 int main()
 {
     float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-    Matrix3f A(data);
-    Matrix3f A_I = A.inverse();
+    SquareMatrix<float, 3> A(data);
+    SquareMatrix<float, 3> A_I = A.inverse();
     float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-    Matrix3f A_I_check(data_check);
+    SquareMatrix<float, 3> A_I_check(data_check);
     (void)A_I;
     assert(A_I == A_I_check);
 
     // stess test
-    static const size_t n = 50;
-    Matrix<float, n, n> A_large;
+    SquareMatrix<float, n_large> A_large;
     A_large.setIdentity();
-    Matrix<float, n, n> A_large_I;
+    SquareMatrix<float, n_large> A_large_I;
     A_large_I.setZero();
 
-    for (size_t i = 0; i < 50; i++) {
+    for (size_t i = 0; i < n_large; i++) {
         A_large_I = A_large.inverse();
         assert(A_large == A_large_I);
     }
diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp
index dfd98e0ae3..44e0e3a055 100644
--- a/test/matrixAssignment.cpp
+++ b/test/matrixAssignment.cpp
@@ -3,6 +3,8 @@
 
 using namespace matrix;
 
+template class Matrix<float, 3, 3>;
+
 int main()
 {
     Matrix3f m;
diff --git a/test/quaternion.cpp b/test/quaternion.cpp
index 92a727d126..1e0ffd3424 100644
--- a/test/quaternion.cpp
+++ b/test/quaternion.cpp
@@ -4,6 +4,11 @@
 
 using namespace matrix;
 
+// instantiate so coverage works
+template class Quaternion<float>;
+template class Euler<float>;
+template class Dcm<float>;
+
 int main()
 {
     // test default ctor
@@ -12,7 +17,7 @@ int main()
     assert(q(1) == 0);
     assert(q(2) == 0);
     assert(q(3) == 0);
-    
+
     q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
     assert(q(0) == 0.1825742f);
     assert(q(1) == 0.3651484f);
diff --git a/test/setIdentity.cpp b/test/setIdentity.cpp
index 0ece759bee..e3b4a2e7dc 100644
--- a/test/setIdentity.cpp
+++ b/test/setIdentity.cpp
@@ -3,6 +3,8 @@
 
 using namespace matrix;
 
+template class Matrix<float, 3, 3>;
+
 int main()
 {
     Matrix3f A;
diff --git a/test/transpose.cpp b/test/transpose.cpp
index 7dd7f2339d..fd466a48c0 100644
--- a/test/transpose.cpp
+++ b/test/transpose.cpp
@@ -4,12 +4,15 @@
 
 using namespace matrix;
 
+template class Matrix<float, 2, 3>;
+template class Matrix<float, 3, 2>;
+
 int main()
 {
-    float data[9] = {1, 2, 3, 4, 5, 6};
+    float data[6] = {1, 2, 3, 4, 5, 6};
     Matrix<float, 2, 3> A(data);
     Matrix<float, 3, 2> A_T = A.transpose();
-    float data_check[9] = {1, 4, 2, 5, 3, 6};
+    float data_check[6] = {1, 4, 2, 5, 3, 6};
     Matrix<float, 3, 2> A_T_check(data_check);
     A_T.print();
     A_T_check.print();
diff --git a/test/vector.cpp b/test/vector.cpp
index a1aabc6a26..c7ee34ec75 100644
--- a/test/vector.cpp
+++ b/test/vector.cpp
@@ -4,6 +4,8 @@
 
 using namespace matrix;
 
+template class Vector<float, 5>;
+
 int main()
 {
     Vector<float, 5> v;
diff --git a/test/vector3.cpp b/test/vector3.cpp
index fe93629e3b..1535efd549 100644
--- a/test/vector3.cpp
+++ b/test/vector3.cpp
@@ -4,6 +4,8 @@
 
 using namespace matrix;
 
+template class Vector<float, 3>;
+
 int main()
 {
     Vector3f a(1, 0, 0);
diff --git a/test/vectorAssignment.cpp b/test/vectorAssignment.cpp
index e5074583dd..9295fc1936 100644
--- a/test/vectorAssignment.cpp
+++ b/test/vectorAssignment.cpp
@@ -3,6 +3,8 @@
 
 using namespace matrix;
 
+template class Vector<float, 3>;
+
 int main()
 {
     Vector3f v;