Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
- Committer:
- NaotoMorita
- Date:
- 2021-11-19
- Revision:
- 71:56c32be982b8
- Parent:
- 70:d12e46fdc2f0
- Child:
- 73:5770a0d470c0
File content as of revision 71:56c32be982b8:
#ifndef __solaESKF_HPP__ #define __solaESKF_HPP__ #include "Matrix.h" #include "MatrixMath.h" #include <cmath> #define M_PI 3.141592f extern Serial twelite; class solaESKF { private: int nState; Matrix pihat; Matrix vihat; Matrix qhat; Matrix accBias; Matrix gyroBias; Matrix gravity; Matrix magBias; float magRadius; Matrix errState; Matrix Phat; Matrix Q; void setDiag(Matrix& mat, float val); void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex); public: solaESKF(); Matrix getPihat(); Matrix getVihat(); Matrix getQhat(); Matrix getAccBias(); Matrix getGyroBias(); Matrix getGravity(); Matrix getMagBias(); float getMagRadius(); Matrix getErrState(); void setPhatPosition(float val); void setPhatVelocity(float val); void setPhatAngleError(float val); void setPhatAccBias(float val); void setPhatGyroBias(float val); void setPhatGravity(float val); void setPhatMagBias(float val); void setPhatMagRadius(float val); void setQVelocity(float val); void setQAngleError(float val); void setQAccBias(float val); void setQGyroBias(float val); void setQMagBias(float val); void setQMagRadius(float val); void updateNominal(Matrix acc,Matrix gyro, float att_dt); void updateErrState(Matrix acc,Matrix gyro, float att_dt); void updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R); void updateImuConstraints(Matrix acc,Matrix mag,Matrix R); void updateOtherConstraints(Matrix mag,float palt,Matrix R); void updateAcc(Matrix acc,Matrix R); void updateMag(Matrix mag,Matrix R); void updateHeading(Matrix mag,Matrix R); void fuseErr2Nominal(); void computeDcm(Matrix& dcm, Matrix quat); void setQhat(float ex,float ey,float ez); void setGravity(float gx,float gy,float gz); void setPihat(float pi_x, float pi_y); Matrix computeAngles(); Matrix quatmultiply(Matrix p, Matrix q); }; #endif //void updateGPSVelocity(Matrix velgps,Matrix R); //void updateGPSPosition(Matrix posgps,Matrix R); //void updateGyroConstraints(Matrix gyro,Matrix R); //void updateMag(Matrix mag,Matrix R); //void updateAccConstraints(Matrix acc,float palt,Matrix R); //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R); //void updateGPSPosition(Matrix posgps,float palt,Matrix R); //