Diff: HAPS_EKF.cpp

Revision:

2:771eed5f655a

Parent:

0:de56252b419e

--- a/HAPS_EKF.cpp	Mon May 31 07:15:13 2021 +0000
+++ b/HAPS_EKF.cpp	Mon May 31 07:27:20 2021 +0000
@@ -148,23 +148,23 @@
     Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
 }
 
-void HAPS_EKF::computeAngles(Vector3& rot, Vector3& rot_g, Vector3 rot_align)
+void HAPS_EKF::computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align)
 {
     float q0 = qhat.getNumber( 1, 1 );
     float q1 = qhat.getNumber( 2, 1 ); 
     float q2 = qhat.getNumber( 3, 1 ); 
     float q3 = qhat.getNumber( 4, 1 ); 
-    rot.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rot_align.x;
-    rot.y = asinf(-2.0f * (q1*q3 - q0*q2))-rot_align.y;
-    rot.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
+    rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
+    rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
     
     q0 = qhat_gyro.getNumber( 1, 1 );
     q1 = qhat_gyro.getNumber( 2, 1 ); 
     q2 = qhat_gyro.getNumber( 3, 1 ); 
     q3 = qhat_gyro.getNumber( 4, 1 ); 
-    rot_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rot_align.x;
-    rot_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rot_align.y;
-    rot_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+    rpy_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
+    rpy_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
+    rpy_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
     
 }