HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.hpp@76:cd1b21936ef1, 2021-11-30 (annotated)

Committer:
NaotoMorita
Date:
Tue Nov 30 07:11:03 2021 +0000
Revision:
76:cd1b21936ef1
Parent:
75:e2c825cdc511
Child:
78:e36a7b844fb5
mod

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