EKF class
Dependencies: MatrixMath Vector3
Revision 2:771eed5f655a, committed 2021-05-31
- Comitter:
- cocorlow
- Date:
- Mon May 31 07:27:20 2021 +0000
- Parent:
- 1:e8b22e2be051
- Commit message:
- rpy
Changed in this revision
HAPS_EKF.cpp | Show annotated file Show diff for this revision Revisions of this file |
HAPS_EKF.hpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r e8b22e2be051 -r 771eed5f655a HAPS_EKF.cpp --- 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); }
diff -r e8b22e2be051 -r 771eed5f655a HAPS_EKF.hpp --- a/HAPS_EKF.hpp Mon May 31 07:15:13 2021 +0000 +++ b/HAPS_EKF.hpp Mon May 31 07:27:20 2021 +0000 @@ -21,7 +21,7 @@ HAPS_EKF(); void updateBetweenMeasures(Vector3 gyro, float att_dt); void updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R); - void computeAngles(Vector3& rot, Vector3& rot_g, Vector3 rot_align); + void computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align); void triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn); Vector3 calcMagRef(Vector3 m); Vector3 calcDynAcc(Vector3 LPacc, Vector3 accref);