HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.hpp@79:365ea9277167, 2021-12-10 (annotated)

Committer:
cocorlow
Date:
Fri Dec 10 10:42:09 2021 +0000
Revision:
79:365ea9277167
Parent:
78:e36a7b844fb5
Child:
80:b241c058df83
Eigen Dense

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