Update Matrix.hpp (#4966)

This was horribly wrong. Matrix is first cast into a matrix of size NxM (which is supposed to be the size of the result - NOT the starting point) so the transpose result becomes garbage. Instead make "Me" an MxN matrix as the original. Took me a whole evening to figure out this problem. Now my Kalman filter finally returns good results.

src/lib/mathlib/math/Matrix.hpp

@@ -305,7 +305,7 @@ public:
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
-		matrix::Matrix<float, N, M> Me(this->arm_mat.pData);
+		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
 		Matrix<N, M> res(Me.transpose().data());
 		return res;
 	}