Extract general functions into utils

EKF/utils.cpp

@@ -0,0 +1,60 @@
+#include "utils.hpp"
+
+matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312)
+{
+		// Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence
+		const float c2 = cosf(rot312(2)); // third rotation is pitch
+		const float s2 = sinf(rot312(2));
+		const float s1 = sinf(rot312(1)); // second rotation is roll
+		const float c1 = cosf(rot312(1));
+		const float s0 = sinf(rot312(0)); // first rotation is yaw
+		const float c0 = cosf(rot312(0));
+
+		matrix::Dcmf R;
+		R(0, 0) = c0 * c2 - s0 * s1 * s2;
+		R(1, 1) = c0 * c1;
+		R(2, 2) = c2 * c1;
+		R(0, 1) = -c1 * s0;
+		R(0, 2) = s2 * c0 + c2 * s1 * s0;
+		R(1, 0) = c2 * s0 + s2 * s1 * c0;
+		R(1, 2) = s0 * s2 - s1 * c0 * c2;
+		R(2, 0) = -s2 * c1;
+		R(2, 1) = s1;
+
+		return R;
+}
+
+float kahanSummation(float sum_previous, float input, float &accumulator)
+{
+	float y = input - accumulator;
+	float t = sum_previous + y;
+	accumulator = (t - sum_previous) - y;
+	return t;
+}
+
+matrix::Dcmf quatToInverseRotMat(const  matrix::Quatf &quat)
+{
+	float q00 = quat(0) * quat(0);
+	float q11 = quat(1) * quat(1);
+	float q22 = quat(2) * quat(2);
+	float q33 = quat(3) * quat(3);
+	float q01 = quat(0) * quat(1);
+	float q02 = quat(0) * quat(2);
+	float q03 = quat(0) * quat(3);
+	float q12 = quat(1) * quat(2);
+	float q13 = quat(1) * quat(3);
+	float q23 = quat(2) * quat(3);
+
+	matrix::Dcmf dcm;
+	dcm(0, 0) = q00 + q11 - q22 - q33;
+	dcm(1, 1) = q00 - q11 + q22 - q33;
+	dcm(2, 2) = q00 - q11 - q22 + q33;
+	dcm(1, 0) = 2.0f * (q12 - q03);
+	dcm(2, 0) = 2.0f * (q13 + q02);
+	dcm(0, 1) = 2.0f * (q12 + q03);
+	dcm(2, 1) = 2.0f * (q23 - q01);
+	dcm(0, 2) = 2.0f * (q13 - q02);
+	dcm(1, 2) = 2.0f * (q23 + q01);
+
+	return dcm;
+}