Merge branch 'feature/attitude' of ssh://git.intern.spaceteamaachen.de:22222/ALPAKA/sta-peak into feature/attitude

include/sta/math/probability/randomVariable.hpp

@@ -0,0 +1,20 @@
+#ifndef STA_MATH_PROBABILITY_RANDOMVARIABLE_HPP
+#define STA_MATH_PROBABILITY_RANDOMVARIABLE_HPP
+
+namespace sta
+{
+namespace math
+{
+namespace probability
+{
+    struct grv
+    {
+        float mean;
+        float variance;
+    };
+}  // namespace probability
+}  // namespace math
+}  // namespace sta
+
+
+#endif //STA_MATH_PROBABILITY_RANDOMVARIABLE_HPP

include/sta/math/quaternion.hpp

@@ -8,6 +8,7 @@
 #ifndef STA_PEAK_QUATERNION_HPP
 #define STA_PEAK_QUATERNION_HPP
 
+#include <sta/math/linalg/matrix.hpp>
 namespace sta
 {
 	namespace math {
@@ -63,17 +64,20 @@ namespace sta
 			Quaternion normalized();
 
 			Quaternion conjugate();			
+
+			matrix toRotationMatrix();
+
+			Quaternion normalized();			
 		public:
 			float x, y, z, w;
 		};
-
-		Quaternion operator-(const Quaternion& q1, const Quaternion& q2);
 		Quaternion operator+(const Quaternion& q1, const Quaternion& q2);
 		Quaternion operator*(const Quaternion& q1, const Quaternion& q2);
 		Quaternion operator*(const Quaternion& quat, float scalar);
 		Quaternion operator*(float scalar, const Quaternion& quat);
 		Quaternion operator*(const Quaternion& quat, double scalar);
 		Quaternion operator*(double scalar, const Quaternion& quat);
+		Quaternion operator-(const Quaternion& q1, const Quaternion& q2);
 	}  // namespace math
 }  // namespace sta
 

src/quaternion.cpp

@@ -54,6 +54,23 @@ namespace sta
 			return Quaternion(qw, qx, qy, qz).normalized();
 	    }
 
+		matrix Quaternion::toRotationMatrix(){
+			
+			matrix R = matrix(3, 3);
+			R.set(0, 0, 2 *(w*w + x*x) - 1);
+			R.set(0, 1, 2 *(x*y - w*z));
+			R.set(0, 2, 2 *(x*z + w*y));
+
+			R.set(1, 0, 2 *(x*y + w*z));
+			R.set(1, 1, 2 *(w*w + y*y) - 1);
+			R.set(1, 2, 2 *(y*z - w*x));
+
+			R.set(2, 0, 2 *(x*z - w*y));
+			R.set(2, 1, 2 *(y*z + w*x));
+			R.set(2, 2, 2 *(w*w + z*z) - 1);
+			return R;
+		}
+
 	    Quaternion Quaternion::unit()
 	    {
 	    	return Quaternion();