diff --git a/src/FDM/LaRCsim/ls_matrix.c b/src/FDM/LaRCsim/ls_matrix.c
index a85d8e773..a25f0edab 100644
--- a/src/FDM/LaRCsim/ls_matrix.c
+++ b/src/FDM/LaRCsim/ls_matrix.c
@@ -223,13 +223,7 @@ int nr_gaussj(double **a, int n, double **b, int m)
 
             if (irow != icol)
                 {
-/*                    for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */
-                        for (l=1;l<=n;l++) 
-                          { 
-                                  temp=a[irow][l]; 
-                                  a[irow][l]=a[icol][l]; 
-                                  a[icol][l]=temp; 
-                          }
+                    for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
                     if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
                 }
             indxr[i] = irow;        /* We are now ready to divide the pivot row */
@@ -245,7 +239,7 @@ int nr_gaussj(double **a, int n, double **b, int m)
                         dum = a[ll][icol];
                         a[ll][icol] = 0.0;
                         for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
-           if (bexists) for (l=1;l<=m;l++) b[ll][i] -= b[icol][l]*dum;
+           if (bexists) for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
                     }
         }
 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index c60458815..79c90055b 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -109,3 +109,7 @@ endmacro()
 
 flightgear_test(test_navs test_navaids2.cxx)
 flightgear_test(test_flightplan test_flightplan.cxx)
+
+add_executable(test_ls_matrix test_ls_matrix.cxx ${CMAKE_SOURCE_DIR}/src/FDM/LaRCsim/ls_matrix.c)
+target_link_libraries(test_ls_matrix SimGearCore)
+add_test(test_ls_matrix ${EXECUTABLE_OUTPUT_PATH}/test_ls_matrix)
diff --git a/tests/test_ls_matrix.cxx b/tests/test_ls_matrix.cxx
new file mode 100644
index 000000000..31faf78f3
--- /dev/null
+++ b/tests/test_ls_matrix.cxx
@@ -0,0 +1,161 @@
+#include <simgear/constants.h>
+#include <simgear/misc/test_macros.hxx>
+extern "C" {
+#include "src/FDM/LaRCsim/ls_matrix.h"
+}
+
+void testCopyMatrix()
+{
+    int nelm = 20;
+    double **src = nr_matrix(1, nelm, 1, nelm);
+    double **dest = nr_matrix(1, nelm, 1, nelm);
+    double invmaxlong = 1.0/(double)RAND_MAX;
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            src[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
+
+    nr_copymat(src, nelm, dest);
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            SG_CHECK_EQUAL(src[i][j], dest[i][j]);
+
+    nr_free_matrix(src, 1, nelm, 1, nelm);
+    nr_free_matrix(dest, 1, nelm, 1, nelm);
+}
+
+void testIdentityMatrix()
+{
+    int nelm = 10;
+    double **id = nr_matrix(1, nelm, 1, nelm);
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            id[i][j] = i == j ? 1.0 : 0.0;
+
+    nr_gaussj(id, nelm, 0, 0);
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            SG_CHECK_EQUAL_EP(id[i][j], i == j ? 1.0 : 0.0);
+
+    nr_free_matrix(id, 1, nelm, 1, nelm);
+}
+
+void testOrthogonalMatrix()
+{
+    int nelm = 3;
+    double **m = nr_matrix(1, nelm, 1, nelm);
+    double **inv = nr_matrix(1, nelm, 1, nelm);
+    double angle = M_PI/3.0;
+
+    m[1][1] = cos(angle);
+    m[1][2] = sin(angle);
+    m[1][3] = 0.0;
+    m[2][1] = -m[1][2];
+    m[2][2] = m[1][1];
+    m[2][3] = 0.0;
+    m[3][1] = 0.0;
+    m[3][2] = 0.0;
+    m[3][3] = 1.0;
+
+    nr_copymat(m, nelm, inv);
+    nr_gaussj(inv, nelm, 0, 0);
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            SG_CHECK_EQUAL_EP(m[i][j], inv[j][i]);
+
+    nr_free_matrix(m, 1, nelm, 1, nelm);
+    nr_free_matrix(inv, 1, nelm, 1, nelm);
+}
+
+void testRandomMatrix()
+{
+    int nelm = 20;
+    double **src = nr_matrix(1, nelm, 1, nelm);
+    double **inv = nr_matrix(1, nelm, 1, nelm);
+    double **id = nr_matrix(1, nelm, 1, nelm);
+    double invmaxlong = 1.0/(double)RAND_MAX;
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            src[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
+
+    nr_copymat(src, nelm, inv);
+    nr_gaussj(inv, nelm, 0, 0);
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j) {
+            id[i][j] = 0.0;
+            for (int k=1; k<=nelm; ++k)
+                id[i][j] += src[i][k]*inv[k][j];
+        }
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            SG_CHECK_EQUAL_EP(id[i][j], i == j ? 1.0 : 0.0);
+
+    nr_free_matrix(src, 1, nelm, 1, nelm);
+    nr_free_matrix(inv, 1, nelm, 1, nelm);
+    nr_free_matrix(id, 1, nelm, 1, nelm);
+}
+
+void testSolveLinearSystem()
+{
+    int nelm = 20;
+    double **src = nr_matrix(1, nelm, 1, nelm);
+    double **inv = nr_matrix(1, nelm, 1, nelm);
+    double **rhs = nr_matrix(1, nelm, 1, 1);
+    double **sol = nr_matrix(1, nelm, 1, 1);
+    double **check = nr_matrix(1, nelm, 1, nelm);
+    double invmaxlong = 1.0/(double)RAND_MAX;
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            src[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
+
+    for (int i=1; i<=nelm; ++i) {
+        rhs[i][1] = 2.0+cos(i*M_PI/nelm);
+        sol[i][1] = rhs[i][1];
+    }
+
+    nr_copymat(src, nelm, inv);
+    nr_gaussj(inv, nelm, sol, 1);
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j) {
+            check[i][j] = 0.0;
+            for (int k=1; k<=nelm; ++k)
+                check[i][j] += src[i][k]*inv[k][j];
+        }
+
+    for (int i=1; i<=nelm; ++i)
+        for (int j=1; j<=nelm; ++j)
+            SG_CHECK_EQUAL_EP(check[i][j], i == j ? 1.0 : 0.0);
+
+    for (int i=1; i<=nelm; ++i) {
+        check[i][1] = 0.0;
+        for (int j=1; j<=nelm; ++j)
+            check[i][1] += src[i][j]*sol[j][1];
+    }
+
+    for (int i=1; i<=nelm; ++i)
+        SG_CHECK_EQUAL_EP(check[i][1], rhs[i][1]);
+
+    nr_free_matrix(src, 1, nelm, 1, nelm);
+    nr_free_matrix(inv, 1, nelm, 1, nelm);
+    nr_free_matrix(check, 1, nelm, 1, nelm);
+    nr_free_matrix(rhs, 1, nelm, 1, 1);
+    nr_free_matrix(sol, 1, nelm, 1, 1);
+}
+
+int main(int argc, char* argv[])
+{
+    testCopyMatrix();
+    testIdentityMatrix();
+    testOrthogonalMatrix();
+    testRandomMatrix();
+    testSolveLinearSystem();
+}