Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp@46:15988dc41923, 2021-10-28 (annotated)
- Committer:
- NaotoMorita
- Date:
- Thu Oct 28 09:44:34 2021 +0000
- Revision:
- 46:15988dc41923
- Parent:
- 45:df4618814803
- Child:
- 47:2467de40951f
newer
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 | 46:15988dc41923 | 34 | void updateGPSVelocity(Matrix velgps,Matrix R); |
NaotoMorita | 46:15988dc41923 | 35 | void updateGPSPosition(Matrix posgps,Matrix R); |
NaotoMorita | 46:15988dc41923 | 36 | void updateGPS(Matrix posgps,Matrix velgps,Matrix R); |
NaotoMorita | 46:15988dc41923 | 37 | void updateAccConstraints(Matrix acc,Matrix R); |
NaotoMorita | 46:15988dc41923 | 38 | void updateGyroConstraints(Matrix gyro,Matrix R); |
NaotoMorita | 44:7d82e63b6a86 | 39 | void fuseErr2Nominal(); |
NaotoMorita | 44:7d82e63b6a86 | 40 | |
NaotoMorita | 44:7d82e63b6a86 | 41 | void computeDcm(Matrix& dcm, Matrix quat); |
NaotoMorita | 44:7d82e63b6a86 | 42 | void setQhat(float ex,float ey,float ez); |
NaotoMorita | 44:7d82e63b6a86 | 43 | void setGravity(float gx,float gy,float gz); |
NaotoMorita | 44:7d82e63b6a86 | 44 | |
NaotoMorita | 44:7d82e63b6a86 | 45 | Matrix computeAngles(); |
NaotoMorita | 44:7d82e63b6a86 | 46 | Matrix quatmultiply(Matrix p, Matrix q); |
NaotoMorita | 44:7d82e63b6a86 | 47 | |
NaotoMorita | 44:7d82e63b6a86 | 48 | }; |
NaotoMorita | 44:7d82e63b6a86 | 49 | |
NaotoMorita | 44:7d82e63b6a86 | 50 | #endif |