Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp@73:5770a0d470c0, 2021-11-21 (annotated)
- Committer:
- NaotoMorita
- Date:
- Sun Nov 21 08:24:40 2021 +0000
- Revision:
- 73:5770a0d470c0
- Parent:
- 71:56c32be982b8
- Child:
- 74:f5fe7fecbd3c
heading fuse
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 | |
NaotoMorita | 44:7d82e63b6a86 | 4 | #include "Matrix.h" |
NaotoMorita | 54:cd514d9d4b19 | 5 | #include "MatrixMath.h" |
NaotoMorita | 54:cd514d9d4b19 | 6 | #include <cmath> |
NaotoMorita | 44:7d82e63b6a86 | 7 | |
NaotoMorita | 44:7d82e63b6a86 | 8 | #define M_PI 3.141592f |
NaotoMorita | 44:7d82e63b6a86 | 9 | |
NaotoMorita | 71:56c32be982b8 | 10 | extern Serial twelite; |
NaotoMorita | 44:7d82e63b6a86 | 11 | |
NaotoMorita | 44:7d82e63b6a86 | 12 | class solaESKF |
NaotoMorita | 44:7d82e63b6a86 | 13 | { |
NaotoMorita | 44:7d82e63b6a86 | 14 | private: |
NaotoMorita | 44:7d82e63b6a86 | 15 | |
NaotoMorita | 44:7d82e63b6a86 | 16 | int nState; |
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 | 62:5519d34eb6e8 | 23 | Matrix magBias; |
NaotoMorita | 62:5519d34eb6e8 | 24 | float magRadius; |
NaotoMorita | 62:5519d34eb6e8 | 25 | |
NaotoMorita | 62:5519d34eb6e8 | 26 | |
NaotoMorita | 44:7d82e63b6a86 | 27 | Matrix errState; |
NaotoMorita | 44:7d82e63b6a86 | 28 | Matrix Phat; |
NaotoMorita | 44:7d82e63b6a86 | 29 | Matrix Q; |
NaotoMorita | 62:5519d34eb6e8 | 30 | |
NaotoMorita | 47:2467de40951f | 31 | void setDiag(Matrix& mat, float val); |
NaotoMorita | 47:2467de40951f | 32 | void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex); |
NaotoMorita | 73:5770a0d470c0 | 33 | float wrap_pi(float x); |
NaotoMorita | 62:5519d34eb6e8 | 34 | |
NaotoMorita | 47:2467de40951f | 35 | public: |
NaotoMorita | 62:5519d34eb6e8 | 36 | |
NaotoMorita | 44:7d82e63b6a86 | 37 | solaESKF(); |
NaotoMorita | 62:5519d34eb6e8 | 38 | |
NaotoMorita | 47:2467de40951f | 39 | Matrix getPihat(); |
NaotoMorita | 47:2467de40951f | 40 | Matrix getVihat(); |
NaotoMorita | 47:2467de40951f | 41 | Matrix getQhat(); |
NaotoMorita | 47:2467de40951f | 42 | Matrix getAccBias(); |
NaotoMorita | 47:2467de40951f | 43 | Matrix getGyroBias(); |
NaotoMorita | 47:2467de40951f | 44 | Matrix getGravity(); |
NaotoMorita | 62:5519d34eb6e8 | 45 | Matrix getMagBias(); |
NaotoMorita | 62:5519d34eb6e8 | 46 | float getMagRadius(); |
NaotoMorita | 47:2467de40951f | 47 | Matrix getErrState(); |
NaotoMorita | 62:5519d34eb6e8 | 48 | |
NaotoMorita | 47:2467de40951f | 49 | void setPhatPosition(float val); |
NaotoMorita | 47:2467de40951f | 50 | void setPhatVelocity(float val); |
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 | 62:5519d34eb6e8 | 55 | void setPhatMagBias(float val); |
NaotoMorita | 62:5519d34eb6e8 | 56 | void setPhatMagRadius(float val); |
NaotoMorita | 47:2467de40951f | 57 | void setQVelocity(float val); |
NaotoMorita | 47:2467de40951f | 58 | void setQAngleError(float val); |
NaotoMorita | 47:2467de40951f | 59 | void setQAccBias(float val); |
NaotoMorita | 47:2467de40951f | 60 | void setQGyroBias(float val); |
NaotoMorita | 62:5519d34eb6e8 | 61 | void setQMagBias(float val); |
NaotoMorita | 62:5519d34eb6e8 | 62 | void setQMagRadius(float val); |
NaotoMorita | 47:2467de40951f | 63 | |
NaotoMorita | 44:7d82e63b6a86 | 64 | void updateNominal(Matrix acc,Matrix gyro, float att_dt); |
NaotoMorita | 44:7d82e63b6a86 | 65 | void updateErrState(Matrix acc,Matrix gyro, float att_dt); |
NaotoMorita | 59:03fe5e16a33c | 66 | void updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R); |
NaotoMorita | 65:c25d7810de44 | 67 | void updateImuConstraints(Matrix acc,Matrix mag,Matrix R); |
NaotoMorita | 65:c25d7810de44 | 68 | void updateOtherConstraints(Matrix mag,float palt,Matrix R); |
NaotoMorita | 61:5e5c4fe12440 | 69 | void updateAcc(Matrix acc,Matrix R); |
NaotoMorita | 70:d12e46fdc2f0 | 70 | void updateMag(Matrix mag,Matrix R); |
NaotoMorita | 73:5770a0d470c0 | 71 | void updateHeading(float a,Matrix R); |
NaotoMorita | 44:7d82e63b6a86 | 72 | void fuseErr2Nominal(); |
NaotoMorita | 44:7d82e63b6a86 | 73 | |
NaotoMorita | 44:7d82e63b6a86 | 74 | void computeDcm(Matrix& dcm, Matrix quat); |
NaotoMorita | 44:7d82e63b6a86 | 75 | void setQhat(float ex,float ey,float ez); |
NaotoMorita | 44:7d82e63b6a86 | 76 | void setGravity(float gx,float gy,float gz); |
NaotoMorita | 44:7d82e63b6a86 | 77 | |
NaotoMorita | 58:93ba28cf5cb3 | 78 | void setPihat(float pi_x, float pi_y); |
osaka | 51:a5af3b280d23 | 79 | |
NaotoMorita | 44:7d82e63b6a86 | 80 | Matrix computeAngles(); |
NaotoMorita | 44:7d82e63b6a86 | 81 | Matrix quatmultiply(Matrix p, Matrix q); |
NaotoMorita | 44:7d82e63b6a86 | 82 | |
NaotoMorita | 44:7d82e63b6a86 | 83 | }; |
NaotoMorita | 44:7d82e63b6a86 | 84 | |
NaotoMorita | 56:c10f1168bd4a | 85 | #endif |
NaotoMorita | 56:c10f1168bd4a | 86 | |
NaotoMorita | 56:c10f1168bd4a | 87 | //void updateGPSVelocity(Matrix velgps,Matrix R); |
NaotoMorita | 56:c10f1168bd4a | 88 | //void updateGPSPosition(Matrix posgps,Matrix R); |
NaotoMorita | 56:c10f1168bd4a | 89 | //void updateGyroConstraints(Matrix gyro,Matrix R); |
NaotoMorita | 56:c10f1168bd4a | 90 | //void updateMag(Matrix mag,Matrix R); |
NaotoMorita | 59:03fe5e16a33c | 91 | //void updateAccConstraints(Matrix acc,float palt,Matrix R); |
NaotoMorita | 59:03fe5e16a33c | 92 | //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R); |
NaotoMorita | 59:03fe5e16a33c | 93 | //void updateGPSPosition(Matrix posgps,float palt,Matrix R); |
NaotoMorita | 61:5e5c4fe12440 | 94 | // |