Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

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?

UserRevisionLine numberNew 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 //