diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 187956ca68..ea9fb9dd08 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -115,6 +115,9 @@ bool                    inverse3x3(float m[], float invOut[]);
 // invOut is an inverted 3x3 matrix when returns true, otherwise matrix is Singular
 bool                    inverse4x4(float m[],float invOut[]);
 
+// matrix multiplication of two NxN matrices
+float* mat_mul(float *A, float *B, uint8_t n);
+
 // see if location is past a line perpendicular to
 // the line between point1 and point2. If point1 is
 // our previous waypoint and point2 is our target waypoint
diff --git a/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp b/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
index fee860739c..0cd045c006 100644
--- a/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
+++ b/libraries/AP_Math/examples/matrix_alg/matrix_alg.cpp
@@ -8,6 +8,8 @@
 #include <stdio.h>
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+#define MAT_ALG_ACCURACY    1e-4f
+
 static uint16_t get_random(void)
 {
     static uint32_t m_z = 1234;
@@ -21,7 +23,7 @@ static uint16_t get_random(void)
 void show_matrix(float *A, int n) {
     for (int i = 0; i < n; i++) {
         for (int j = 0; j < n; j++)
-            printf("%2.5f  ", A[i * n + j]);
+            printf("%.10f  ", A[i * n + j]);
         printf("\n");
     }
 }
@@ -30,7 +32,7 @@ bool compare_mat(float *A, float *B, uint8_t n)
 {
     for(uint8_t i = 0; i < n; i++) {
         for(uint8_t j = 0; j < n; j++) {
-            if(fabsf(A[i*n + j] - B[i*n + j]) > 1e-4f) {
+            if(fabsf(A[i*n + j] - B[i*n + j]) > MAT_ALG_ACCURACY) {
                 return false;
             }
         }
@@ -41,16 +43,21 @@ bool compare_mat(float *A, float *B, uint8_t n)
 static void test_matrix_inverse(void)
 {
     //fast inverses
-    float test_mat[25];
+    float test_mat[25],ident_mat[25];
+    float *out_mat;
     for(uint8_t i = 0;i<25;i++) {
         test_mat[i] = pow(-1,i)*get_random()/0.7f;
     }
     float mat[25];
-    uint8_t l = 0;
+
 
     //Test for 3x3 matrix
-    l = 0;
+    memset(ident_mat,0,sizeof(ident_mat));
+    for(uint8_t i=0; i<3; i++) {
+        ident_mat[i*3+i] = 1.0f;
+    }
     if(inverse(test_mat,mat,3)){
+        out_mat = mat_mul(test_mat,mat,3);
         inverse(mat,mat,3);
     } else {
         hal.console->printf("3x3 Matrix is Singular!\n");
@@ -61,38 +68,60 @@ static void test_matrix_inverse(void)
     show_matrix(test_mat,3);
     printf("\nInverse of Inverse of matrix\n");
     show_matrix(mat,3);
+    printf("\nInv(A) * A\n");
+    show_matrix(out_mat,3);
     printf("\n");
     //compare matrix
     if(!compare_mat(test_mat,mat,3)) {
         printf("Test Failed!!\n");
         return;
     }
+    if(!compare_mat(ident_mat,out_mat,3)) {
+        printf("Identity output Test Failed!!\n");
+        return;
+    }
+
 
     //Test for 4x4 matrix
-    l = 0;
+    memset(ident_mat,0,sizeof(ident_mat));
+    for(uint8_t i=0; i<4; i++) {
+        ident_mat[i*4+i] = 1.0f;
+    }
     if(inverse(test_mat,mat,4)){
+        out_mat = mat_mul(test_mat,mat,4);
         inverse(mat,mat,4);
     } else {
-        hal.console->printf("3x3 Matrix is Singular!\n");
+        hal.console->printf("4x4 Matrix is Singular!\n");
         return;
-
     }
     printf("\n\n4x4 Test Matrix:\n");
     show_matrix(test_mat,4);
     printf("\nInverse of Inverse of matrix\n");
     show_matrix(mat,4);
+    printf("\nInv(A) * A\n");
+    show_matrix(out_mat,4);
     printf("\n");
     if(!compare_mat(test_mat,mat,4)) {
         printf("Test Failed!!\n");
         return;
     }
+    if(!compare_mat(ident_mat,out_mat,4)) {
+        printf("Identity output Test Failed!!\n");
+        return;
+    }
+
+
 
     //Test for 5x5 matrix
-    l = 0;
+    memset(ident_mat,0,sizeof(ident_mat));
+    for(uint8_t i=0; i<5; i++) {
+        ident_mat[i*5+i] = 1.0f;
+    }
     if(inverse(test_mat,mat,5)) {
+        out_mat = mat_mul(test_mat,mat,5);
         inverse(mat,mat,5);
     } else {
-        hal.console->printf("3x3 Matrix is Singular!\n");
+        hal.console->printf("5x5 Matrix is Singular!\n");
         return;
 
     }
@@ -101,12 +130,17 @@ static void test_matrix_inverse(void)
     show_matrix(test_mat,5);
     printf("\nInverse of Inverse of matrix\n");
     show_matrix(mat,5);
+    printf("\nInv(A) * A\n");
+    show_matrix(out_mat,5);
     printf("\n");
     if(!compare_mat(test_mat,mat,5)) {
         printf("Test Failed!!\n");
         return;
     }
-
+    if(!compare_mat(ident_mat,out_mat,5)) {
+        printf("Identity output Test Failed!!\n");
+        return;
+    }
 
     hal.console->printf("All tests succeeded!!\n");
 }
diff --git a/libraries/AP_Math/matrix_alg.cpp b/libraries/AP_Math/matrix_alg.cpp
index 997f0f34b3..f1ee07ca34 100644
--- a/libraries/AP_Math/matrix_alg.cpp
+++ b/libraries/AP_Math/matrix_alg.cpp
@@ -23,6 +23,8 @@
 #include <stdio.h>
 extern const AP_HAL::HAL& hal;
 
+//TODO: use higher precision datatypes to achieve more accuracy for matrix algebra operations
+
 /*
  *    Does matrix multiplication of two regular/square matrices
  *