HAPSRG / solaESKF_Eigen

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?

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