HAPSRG / solaESKF_Eigen

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?

UserRevisionLine numberNew 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