Features and fixes

* added the trace function for a SquareMatrix

* added Vector3.hat() and it's counterpart Dcm.vee()
for skewsymskew symmetric matrix operations in relation to the cross product
see https://en.wikipedia.org/wiki/Hat_operator

* Matrix::write_string produced runtime errors when I used it in PX4 posix simulation, i simplified it

* a Matrix3f is a SquareMatrix

* added tests for SquareMatrix.trace, Vector3.hat and Dcm.vee

* added a test for quaternion initialisation from array

* preventing buffer overflows in Matrix.write_string method

matrix/SquareMatrix.hpp

@@ -61,6 +61,17 @@ public:
         return res;
     }
 
+    Type trace() const
+    {
+        Type res = 0;
+        const SquareMatrix<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res += self(i, i);
+        }
+        return res;
+    }
+
 };
 
 typedef SquareMatrix<float, 3> SquareMatrix3f;
@@ -215,7 +226,7 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
     return X;
 }
 
-
+typedef SquareMatrix<float, 3> Matrix3f;
 
 } // namespace matrix