HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.hpp@47:2467de40951f, 2021-10-29 (annotated)

Committer:
NaotoMorita
Date:
Fri Oct 29 13:30:03 2021 +0000
Revision:
47:2467de40951f
Parent:
46:15988dc41923
Child:
50:dadad0567349
Huge Bug Fix

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 44:7d82e63b6a86 5
NaotoMorita 44:7d82e63b6a86 6 #define M_PI 3.141592f
NaotoMorita 44:7d82e63b6a86 7
NaotoMorita 44:7d82e63b6a86 8
NaotoMorita 44:7d82e63b6a86 9 class solaESKF
NaotoMorita 44:7d82e63b6a86 10 {
NaotoMorita 44:7d82e63b6a86 11 private:
NaotoMorita 44:7d82e63b6a86 12
NaotoMorita 44:7d82e63b6a86 13 int nState;
NaotoMorita 44:7d82e63b6a86 14 Matrix pihat;
NaotoMorita 44:7d82e63b6a86 15 Matrix vihat;
NaotoMorita 44:7d82e63b6a86 16 Matrix qhat;
NaotoMorita 44:7d82e63b6a86 17 Matrix accBias;
NaotoMorita 44:7d82e63b6a86 18 Matrix gyroBias;
NaotoMorita 44:7d82e63b6a86 19 Matrix gravity;
NaotoMorita 44:7d82e63b6a86 20
NaotoMorita 44:7d82e63b6a86 21 Matrix errState;
NaotoMorita 44:7d82e63b6a86 22 Matrix Phat;
NaotoMorita 44:7d82e63b6a86 23 Matrix Q;
NaotoMorita 44:7d82e63b6a86 24
NaotoMorita 47:2467de40951f 25 void setDiag(Matrix& mat, float val);
NaotoMorita 47:2467de40951f 26 void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex);
NaotoMorita 47:2467de40951f 27
NaotoMorita 47:2467de40951f 28 public:
NaotoMorita 47:2467de40951f 29
NaotoMorita 44:7d82e63b6a86 30 solaESKF();
NaotoMorita 44:7d82e63b6a86 31
NaotoMorita 47:2467de40951f 32 Matrix getPihat();
NaotoMorita 47:2467de40951f 33 Matrix getVihat();
NaotoMorita 47:2467de40951f 34 Matrix getQhat();
NaotoMorita 47:2467de40951f 35 Matrix getAccBias();
NaotoMorita 47:2467de40951f 36 Matrix getGyroBias();
NaotoMorita 47:2467de40951f 37 Matrix getGravity();
NaotoMorita 47:2467de40951f 38 Matrix getErrState();
NaotoMorita 47:2467de40951f 39
NaotoMorita 47:2467de40951f 40 void setPhatPosition(float val);
NaotoMorita 47:2467de40951f 41 void setPhatVelocity(float val);
NaotoMorita 47:2467de40951f 42 void setPhatAngleError(float val);
NaotoMorita 47:2467de40951f 43 void setPhatAccBias(float val);
NaotoMorita 47:2467de40951f 44 void setPhatGyroBias(float val);
NaotoMorita 47:2467de40951f 45 void setPhatGravity(float val);
NaotoMorita 47:2467de40951f 46 void setQVelocity(float val);
NaotoMorita 47:2467de40951f 47 void setQAngleError(float val);
NaotoMorita 47:2467de40951f 48 void setQAccBias(float val);
NaotoMorita 47:2467de40951f 49 void setQGyroBias(float val);
NaotoMorita 47:2467de40951f 50
NaotoMorita 44:7d82e63b6a86 51 void updateNominal(Matrix acc,Matrix gyro, float att_dt);
NaotoMorita 44:7d82e63b6a86 52 void updateErrState(Matrix acc,Matrix gyro, float att_dt);
NaotoMorita 46:15988dc41923 53 void updateGPSVelocity(Matrix velgps,Matrix R);
NaotoMorita 46:15988dc41923 54 void updateGPSPosition(Matrix posgps,Matrix R);
NaotoMorita 46:15988dc41923 55 void updateGPS(Matrix posgps,Matrix velgps,Matrix R);
NaotoMorita 46:15988dc41923 56 void updateAccConstraints(Matrix acc,Matrix R);
NaotoMorita 46:15988dc41923 57 void updateGyroConstraints(Matrix gyro,Matrix R);
NaotoMorita 44:7d82e63b6a86 58 void fuseErr2Nominal();
NaotoMorita 44:7d82e63b6a86 59
NaotoMorita 44:7d82e63b6a86 60 void computeDcm(Matrix& dcm, Matrix quat);
NaotoMorita 44:7d82e63b6a86 61 void setQhat(float ex,float ey,float ez);
NaotoMorita 44:7d82e63b6a86 62 void setGravity(float gx,float gy,float gz);
NaotoMorita 44:7d82e63b6a86 63
NaotoMorita 44:7d82e63b6a86 64 Matrix computeAngles();
NaotoMorita 44:7d82e63b6a86 65 Matrix quatmultiply(Matrix p, Matrix q);
NaotoMorita 44:7d82e63b6a86 66
NaotoMorita 44:7d82e63b6a86 67 };
NaotoMorita 44:7d82e63b6a86 68
NaotoMorita 44:7d82e63b6a86 69 #endif