HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.hpp@86:cc2c0025d820, 18 months ago (annotated)

Committer:
NaotoMorita
Date:
Tue Dec 20 06:24:27 2022 +0000
Revision:
86:cc2c0025d820
Parent:
84:fb14acabefd3
Child:
87:6862abea60ed
vector2ned

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
NaotoMorita 80:b241c058df83 31 void setDiag(Matrix3f& mat, float val);
cocorlow 78:e36a7b844fb5 32 void setDiag(MatrixXf& mat, float val);
cocorlow 78:e36a7b844fb5 33 void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex);
NaotoMorita 62:5519d34eb6e8 34
NaotoMorita 47:2467de40951f 35 public:
NaotoMorita 62:5519d34eb6e8 36
NaotoMorita 44:7d82e63b6a86 37 solaESKF();
NaotoMorita 62:5519d34eb6e8 38
cocorlow 78:e36a7b844fb5 39 Vector3f getPihat();
cocorlow 78:e36a7b844fb5 40 Vector3f getVihat();
cocorlow 78:e36a7b844fb5 41 Vector4f getQhat();
cocorlow 78:e36a7b844fb5 42 Vector3f getAccBias();
cocorlow 78:e36a7b844fb5 43 Vector3f getGyroBias();
cocorlow 78:e36a7b844fb5 44 Vector3f getGravity();
cocorlow 78:e36a7b844fb5 45 VectorXf getErrState();
NaotoMorita 80:b241c058df83 46 VectorXf getState();
NaotoMorita 80:b241c058df83 47 VectorXf getVariance();
NaotoMorita 62:5519d34eb6e8 48
NaotoMorita 80:b241c058df83 49 void setPhatPosition(float valNE,float valD);
NaotoMorita 80:b241c058df83 50 void setPhatVelocity(float valNE,float valD);
NaotoMorita 47:2467de40951f 51 void setPhatAngleError(float val);
NaotoMorita 47:2467de40951f 52 void setPhatAccBias(float val);
NaotoMorita 47:2467de40951f 53 void setPhatGyroBias(float val);
NaotoMorita 47:2467de40951f 54 void setPhatGravity(float val);
NaotoMorita 80:b241c058df83 55 void setQVelocity(float valNE,float valD);
NaotoMorita 47:2467de40951f 56 void setQAngleError(float val);
NaotoMorita 47:2467de40951f 57 void setQAccBias(float val);
NaotoMorita 47:2467de40951f 58 void setQGyroBias(float val);
NaotoMorita 47:2467de40951f 59
cocorlow 78:e36a7b844fb5 60 void updateNominal(Vector3f acc, Vector3f gyro, float att_dt);
cocorlow 78:e36a7b844fb5 61 void updateErrState(Vector3f acc, Vector3f gyro, float att_dt);
cocorlow 78:e36a7b844fb5 62 void updateGPS(Vector3f posgps,float palt, Vector3f velgps, MatrixXf R);
cocorlow 78:e36a7b844fb5 63 void updateGPSPosition(Vector3f posgps, float palt, Matrix3f R);
cocorlow 78:e36a7b844fb5 64 void updateGPSVelocity(Vector3f velgps, Matrix3f R);
cocorlow 78:e36a7b844fb5 65 void updateAcc(Vector3f acc, Matrix3f R);
cocorlow 79:365ea9277167 66 void updateHeading(float a, Matrix<float, 1, 1> R);
NaotoMorita 83:8f6ae61d47ac 67 void updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R);
NaotoMorita 80:b241c058df83 68 void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R);
NaotoMorita 44:7d82e63b6a86 69 void fuseErr2Nominal();
NaotoMorita 84:fb14acabefd3 70 void fuseErr2Nominal(VectorXf errVal);
NaotoMorita 44:7d82e63b6a86 71
cocorlow 78:e36a7b844fb5 72 void computeDcm(Matrix3f& dcm, Vector4f quat);
NaotoMorita 44:7d82e63b6a86 73 void setQhat(float ex,float ey,float ez);
NaotoMorita 44:7d82e63b6a86 74 void setGravity(float gx,float gy,float gz);
cocorlow 78:e36a7b844fb5 75 Vector3f calcDynAcc(Vector3f acc);
NaotoMorita 86:cc2c0025d820 76 Vector3f vector2Body(Vector3f veci);
NaotoMorita 86:cc2c0025d820 77 Vector3f vector2NED(Vector3f vecb);
NaotoMorita 44:7d82e63b6a86 78
NaotoMorita 81:230a3d2b0493 79 void setPihat(float pi_x, float pi_y,float pi_z);
osaka 51:a5af3b280d23 80
cocorlow 78:e36a7b844fb5 81 Vector3f computeAngles();
cocorlow 78:e36a7b844fb5 82 Vector4f quatmultiply(Vector4f p, Vector4f q);
NaotoMorita 44:7d82e63b6a86 83
cocorlow 78:e36a7b844fb5 84 static Matrix3f Matrixcross(Vector3f v);
NaotoMorita 44:7d82e63b6a86 85 };
NaotoMorita 44:7d82e63b6a86 86
NaotoMorita 56:c10f1168bd4a 87 #endif
NaotoMorita 56:c10f1168bd4a 88
NaotoMorita 56:c10f1168bd4a 89 //void updateGPSVelocity(Matrix velgps,Matrix R);
NaotoMorita 56:c10f1168bd4a 90 //void updateGPSPosition(Matrix posgps,Matrix R);
NaotoMorita 56:c10f1168bd4a 91 //void updateGyroConstraints(Matrix gyro,Matrix R);
NaotoMorita 56:c10f1168bd4a 92 //void updateMag(Matrix mag,Matrix R);
NaotoMorita 59:03fe5e16a33c 93 //void updateAccConstraints(Matrix acc,float palt,Matrix R);
NaotoMorita 59:03fe5e16a33c 94 //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R);
NaotoMorita 59:03fe5e16a33c 95 //void updateGPSPosition(Matrix posgps,float palt,Matrix R);
NaotoMorita 61:5e5c4fe12440 96 //