Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 84:fb14acabefd3
- Parent:
- 83:8f6ae61d47ac
- Child:
- 85:552fe30e7f51
- Child:
- 86:cc2c0025d820
diff -r 8f6ae61d47ac -r fb14acabefd3 solaESKF.cpp --- a/solaESKF.cpp Wed Jun 29 07:56:42 2022 +0000 +++ b/solaESKF.cpp Wed Jul 13 05:08:01 2022 +0000 @@ -556,6 +556,40 @@ errState = VectorXf::Zero(nState); } +void solaESKF::fuseErr2Nominal(VectorXf errVal) +{ + //position + pihat(0) += errVal(0); + pihat(1) += errVal(1); + pihat(2) += errVal(2); + + //velocity + vihat(0) += errVal(3); + vihat(1) += errVal(4); + vihat(2) += errVal(5); + + //angle error + Vector4f qerr; + qerr << 1.0f, 0.5f*errVal(6), 0.5f*errVal(7), 0.5f*errVal(8); + qhat = quatmultiply(qhat, qerr); + qhat.normalize(); + + //acc bias + accBias(0) += errVal(9); + accBias(1) += errVal(10); + accBias(2) += errVal(11); + + //gyro bias + gyroBias(0) += errVal(12); + gyroBias(1) += errVal(13); + gyroBias(2) += errVal(14); + + //gravity bias + //gravity(0) += errVal(15); + //gravity(1) += errVal(16); + //gravity(2) += errVal(17); +} + Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q) { Vector4f qout;