Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.hpp
- Revision:
- 62:5519d34eb6e8
- Parent:
- 61:5e5c4fe12440
- Child:
- 65:c25d7810de44
diff -r 5e5c4fe12440 -r 5519d34eb6e8 solaESKF.hpp --- a/solaESKF.hpp Tue Nov 16 13:56:44 2021 +0000 +++ b/solaESKF.hpp Tue Nov 16 14:17:40 2021 +0000 @@ -20,37 +20,45 @@ Matrix accBias; Matrix gyroBias; Matrix gravity; - - + Matrix magBias; + float magRadius; + + Matrix errState; Matrix Phat; Matrix Q; - + void setDiag(Matrix& mat, float val); void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex); - + public: - + solaESKF(); - + Matrix getPihat(); Matrix getVihat(); Matrix getQhat(); Matrix getAccBias(); Matrix getGyroBias(); Matrix getGravity(); + Matrix getMagBias(); + float getMagRadius(); Matrix getErrState(); - + void setPhatPosition(float val); void setPhatVelocity(float val); void setPhatAngleError(float val); void setPhatAccBias(float val); void setPhatGyroBias(float val); void setPhatGravity(float val); + void setPhatMagBias(float val); + void setPhatMagRadius(float val); void setQVelocity(float val); void setQAngleError(float val); void setQAccBias(float val); void setQGyroBias(float val); + void setQMagBias(float val); + void setQMagRadius(float val); void updateNominal(Matrix acc,Matrix gyro, float att_dt); void updateErrState(Matrix acc,Matrix gyro, float att_dt);