Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
- Committer:
- NaotoMorita
- Date:
- 2021-10-26
- Revision:
- 44:7d82e63b6a86
- Child:
- 45:df4618814803
File content as of revision 44:7d82e63b6a86:
#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