Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.hpp
- Revision:
- 44:7d82e63b6a86
- Child:
- 45:df4618814803
diff -r cbc2c2d65131 -r 7d82e63b6a86 solaESKF.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/solaESKF.hpp Tue Oct 26 05:33:11 2021 +0000 @@ -0,0 +1,46 @@ +#ifndef __solaESKF_HPP__ +#define __solaESKF_HPP__ + +#include "Matrix.h" + +#define M_PI 3.141592f + + +class solaESKF +{ +private: + + int nState; + +public: + + Matrix pihat; + Matrix vihat; + Matrix qhat; + Matrix accBias; + Matrix gyroBias; + Matrix gravity; + + Matrix errState; + Matrix Phat; + Matrix Q; + + solaESKF(); + + void setDiag(Matrix& mat, float val); + void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndez); + void updateNominal(Matrix acc,Matrix gyro, float att_dt); + void updateErrState(Matrix acc,Matrix gyro, float att_dt); + + void fuseErr2Nominal(); + + void computeDcm(Matrix& dcm, Matrix quat); + void setQhat(float ex,float ey,float ez); + void setGravity(float gx,float gy,float gz); + + Matrix computeAngles(); + Matrix quatmultiply(Matrix p, Matrix q); + +}; + +#endif \ No newline at end of file