Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp@87:6862abea60ed, 20 months ago (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Dec 23 07:35:45 2022 +0000
- Revision:
- 87:6862abea60ed
- Parent:
- 86:cc2c0025d820
fuse center 2 nominal
Who changed what in which revision?
User | Revision | Line number | New 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 | 87:6862abea60ed | 70 | void fuseCenter2Nominal(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 | // |