Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp@45:df4618814803, 2021-10-26 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Oct 26 05:36:11 2021 +0000
- Revision:
- 45:df4618814803
- Parent:
- 44:7d82e63b6a86
- Child:
- 46:15988dc41923
solaeskf compiled
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NaotoMorita | 44:7d82e63b6a86 | 1 | #ifndef __solaESKF_HPP__ |
NaotoMorita | 44:7d82e63b6a86 | 2 | #define __solaESKF_HPP__ |
NaotoMorita | 44:7d82e63b6a86 | 3 | |
NaotoMorita | 44:7d82e63b6a86 | 4 | #include "Matrix.h" |
NaotoMorita | 44:7d82e63b6a86 | 5 | |
NaotoMorita | 44:7d82e63b6a86 | 6 | #define M_PI 3.141592f |
NaotoMorita | 44:7d82e63b6a86 | 7 | |
NaotoMorita | 44:7d82e63b6a86 | 8 | |
NaotoMorita | 44:7d82e63b6a86 | 9 | class solaESKF |
NaotoMorita | 44:7d82e63b6a86 | 10 | { |
NaotoMorita | 44:7d82e63b6a86 | 11 | private: |
NaotoMorita | 44:7d82e63b6a86 | 12 | |
NaotoMorita | 44:7d82e63b6a86 | 13 | int nState; |
NaotoMorita | 44:7d82e63b6a86 | 14 | |
NaotoMorita | 44:7d82e63b6a86 | 15 | public: |
NaotoMorita | 44:7d82e63b6a86 | 16 | |
NaotoMorita | 44:7d82e63b6a86 | 17 | Matrix pihat; |
NaotoMorita | 44:7d82e63b6a86 | 18 | Matrix vihat; |
NaotoMorita | 44:7d82e63b6a86 | 19 | Matrix qhat; |
NaotoMorita | 44:7d82e63b6a86 | 20 | Matrix accBias; |
NaotoMorita | 44:7d82e63b6a86 | 21 | Matrix gyroBias; |
NaotoMorita | 44:7d82e63b6a86 | 22 | Matrix gravity; |
NaotoMorita | 44:7d82e63b6a86 | 23 | |
NaotoMorita | 44:7d82e63b6a86 | 24 | Matrix errState; |
NaotoMorita | 44:7d82e63b6a86 | 25 | Matrix Phat; |
NaotoMorita | 44:7d82e63b6a86 | 26 | Matrix Q; |
NaotoMorita | 44:7d82e63b6a86 | 27 | |
NaotoMorita | 44:7d82e63b6a86 | 28 | solaESKF(); |
NaotoMorita | 44:7d82e63b6a86 | 29 | |
NaotoMorita | 44:7d82e63b6a86 | 30 | void setDiag(Matrix& mat, float val); |
NaotoMorita | 45:df4618814803 | 31 | void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex); |
NaotoMorita | 44:7d82e63b6a86 | 32 | void updateNominal(Matrix acc,Matrix gyro, float att_dt); |
NaotoMorita | 44:7d82e63b6a86 | 33 | void updateErrState(Matrix acc,Matrix gyro, float att_dt); |
NaotoMorita | 44:7d82e63b6a86 | 34 | |
NaotoMorita | 44:7d82e63b6a86 | 35 | void fuseErr2Nominal(); |
NaotoMorita | 44:7d82e63b6a86 | 36 | |
NaotoMorita | 44:7d82e63b6a86 | 37 | void computeDcm(Matrix& dcm, Matrix quat); |
NaotoMorita | 44:7d82e63b6a86 | 38 | void setQhat(float ex,float ey,float ez); |
NaotoMorita | 44:7d82e63b6a86 | 39 | void setGravity(float gx,float gy,float gz); |
NaotoMorita | 44:7d82e63b6a86 | 40 | |
NaotoMorita | 44:7d82e63b6a86 | 41 | Matrix computeAngles(); |
NaotoMorita | 44:7d82e63b6a86 | 42 | Matrix quatmultiply(Matrix p, Matrix q); |
NaotoMorita | 44:7d82e63b6a86 | 43 | |
NaotoMorita | 44:7d82e63b6a86 | 44 | }; |
NaotoMorita | 44:7d82e63b6a86 | 45 | |
NaotoMorita | 44:7d82e63b6a86 | 46 | #endif |