Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 82:662b810155e9
- Parent:
- 81:230a3d2b0493
- Child:
- 83:8f6ae61d47ac
diff -r 230a3d2b0493 -r 662b810155e9 solaESKF.cpp --- a/solaESKF.cpp Fri Jun 24 09:10:18 2022 +0000 +++ b/solaESKF.cpp Wed Jun 29 01:39:26 2022 +0000 @@ -613,6 +613,17 @@ return dynAcc; } +Vector3f solaESKF::calcDynAcc(Vector3f acc) +{ + Vector3f accm = acc - accBias; + Matrix3f dcm; + computeDcm(dcm, qhat); + Matrix3f tdcm = dcm.transpose(); + + Vector3f Vb = tdcm*vihat; + return dynAcc; +} + void solaESKF::setGravity(float gx,float gy,float gz) { @@ -758,7 +769,7 @@ { pihat(0) = pi_x; pihat(1) = pi_y; - pihat(3,1) = pi_z; + pihat(2) = pi_z; } Matrix3f solaESKF::Matrixcross(Vector3f v)