HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.hpp@78:e36a7b844fb5, 2021-12-06 (annotated)

Committer:
cocorlow
Date:
Mon Dec 06 08:24:13 2021 +0000
Revision:
78:e36a7b844fb5
Parent:
76:cd1b21936ef1
Child:
79:365ea9277167
Eigen

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
cocorlow 78:e36a7b844fb5 4 #include <Eigen/Core.h>
cocorlow 78:e36a7b844fb5 5 #include <Eigen/LU.h>
NaotoMorita 54:cd514d9d4b19 6 #include <cmath>
NaotoMorita 44:7d82e63b6a86 7
cocorlow 78:e36a7b844fb5 8 using namespace Eigen;
cocorlow 78:e36a7b844fb5 9
cocorlow 78:e36a7b844fb5 10 #ifndef M_PI_F
cocorlow 78:e36a7b844fb5 11 #define M_PI_F 3.141592f
cocorlow 78:e36a7b844fb5 12 #endif
NaotoMorita 44:7d82e63b6a86 13
NaotoMorita 44:7d82e63b6a86 14 class solaESKF
NaotoMorita 44:7d82e63b6a86 15 {
NaotoMorita 44:7d82e63b6a86 16 private:
NaotoMorita 44:7d82e63b6a86 17
NaotoMorita 44:7d82e63b6a86 18 int nState;
cocorlow 78:e36a7b844fb5 19 Vector3f pihat;
cocorlow 78:e36a7b844fb5 20 Vector3f vihat;
cocorlow 78:e36a7b844fb5 21 Vector4f qhat;
cocorlow 78:e36a7b844fb5 22 Vector3f accBias;
cocorlow 78:e36a7b844fb5 23 Vector3f gyroBias;
cocorlow 78:e36a7b844fb5 24 Vector3f gravity;
NaotoMorita 62:5519d34eb6e8 25
cocorlow 78:e36a7b844fb5 26 VectorXf errState;
cocorlow 78:e36a7b844fb5 27 MatrixXf Phat;
cocorlow 78:e36a7b844fb5 28 MatrixXf Q;
NaotoMorita 62:5519d34eb6e8 29
cocorlow 78:e36a7b844fb5 30 void setDiag(MatrixXf& mat, float val);
cocorlow 78:e36a7b844fb5 31 void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex);
NaotoMorita 62:5519d34eb6e8 32
NaotoMorita 47:2467de40951f 33 public:
NaotoMorita 62:5519d34eb6e8 34
NaotoMorita 44:7d82e63b6a86 35 solaESKF();
NaotoMorita 62:5519d34eb6e8 36
cocorlow 78:e36a7b844fb5 37 Vector3f getPihat();
cocorlow 78:e36a7b844fb5 38 Vector3f getVihat();
cocorlow 78:e36a7b844fb5 39 Vector4f getQhat();
cocorlow 78:e36a7b844fb5 40 Vector3f getAccBias();
cocorlow 78:e36a7b844fb5 41 Vector3f getGyroBias();
cocorlow 78:e36a7b844fb5 42 Vector3f getGravity();
cocorlow 78:e36a7b844fb5 43 VectorXf getErrState();
NaotoMorita 62:5519d34eb6e8 44
NaotoMorita 47:2467de40951f 45 void setPhatPosition(float val);
NaotoMorita 47:2467de40951f 46 void setPhatVelocity(float val);
NaotoMorita 47:2467de40951f 47 void setPhatAngleError(float val);
NaotoMorita 47:2467de40951f 48 void setPhatAccBias(float val);
NaotoMorita 47:2467de40951f 49 void setPhatGyroBias(float val);
NaotoMorita 47:2467de40951f 50 void setPhatGravity(float val);
NaotoMorita 47:2467de40951f 51 void setQVelocity(float val);
NaotoMorita 47:2467de40951f 52 void setQAngleError(float val);
NaotoMorita 47:2467de40951f 53 void setQAccBias(float val);
NaotoMorita 47:2467de40951f 54 void setQGyroBias(float val);
NaotoMorita 47:2467de40951f 55
cocorlow 78:e36a7b844fb5 56 void updateNominal(Vector3f acc, Vector3f gyro, float att_dt);
cocorlow 78:e36a7b844fb5 57 void updateErrState(Vector3f acc, Vector3f gyro, float att_dt);
cocorlow 78:e36a7b844fb5 58 void updateGPS(Vector3f posgps,float palt, Vector3f velgps, MatrixXf R);
cocorlow 78:e36a7b844fb5 59 void updateGPSPosition(Vector3f posgps, float palt, Matrix3f R);
cocorlow 78:e36a7b844fb5 60 void updateGPSVelocity(Vector3f velgps, Matrix3f R);
cocorlow 78:e36a7b844fb5 61 void updateAcc(Vector3f acc, Matrix3f R);
cocorlow 78:e36a7b844fb5 62 void updateHeading(float a, Matrix3f R);
NaotoMorita 44:7d82e63b6a86 63 void fuseErr2Nominal();
NaotoMorita 44:7d82e63b6a86 64
cocorlow 78:e36a7b844fb5 65 void computeDcm(Matrix3f& dcm, Vector4f quat);
NaotoMorita 44:7d82e63b6a86 66 void setQhat(float ex,float ey,float ez);
NaotoMorita 44:7d82e63b6a86 67 void setGravity(float gx,float gy,float gz);
cocorlow 78:e36a7b844fb5 68 Vector3f calcDynAcc(Vector3f acc);
NaotoMorita 44:7d82e63b6a86 69
NaotoMorita 58:93ba28cf5cb3 70 void setPihat(float pi_x, float pi_y);
osaka 51:a5af3b280d23 71
cocorlow 78:e36a7b844fb5 72 Vector3f computeAngles();
cocorlow 78:e36a7b844fb5 73 Vector4f quatmultiply(Vector4f p, Vector4f q);
NaotoMorita 44:7d82e63b6a86 74
cocorlow 78:e36a7b844fb5 75 static Matrix3f Matrixcross(Vector3f v);
NaotoMorita 44:7d82e63b6a86 76 };
NaotoMorita 44:7d82e63b6a86 77
NaotoMorita 56:c10f1168bd4a 78 #endif
NaotoMorita 56:c10f1168bd4a 79
NaotoMorita 56:c10f1168bd4a 80 //void updateGPSVelocity(Matrix velgps,Matrix R);
NaotoMorita 56:c10f1168bd4a 81 //void updateGPSPosition(Matrix posgps,Matrix R);
NaotoMorita 56:c10f1168bd4a 82 //void updateGyroConstraints(Matrix gyro,Matrix R);
NaotoMorita 56:c10f1168bd4a 83 //void updateMag(Matrix mag,Matrix R);
NaotoMorita 59:03fe5e16a33c 84 //void updateAccConstraints(Matrix acc,float palt,Matrix R);
NaotoMorita 59:03fe5e16a33c 85 //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R);
NaotoMorita 59:03fe5e16a33c 86 //void updateGPSPosition(Matrix posgps,float palt,Matrix R);
NaotoMorita 61:5e5c4fe12440 87 //