EKF class
Dependencies: MatrixMath Vector3
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); }