Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
- Committer:
- NaotoMorita
- Date:
- 2021-10-29
- Revision:
- 47:2467de40951f
- Parent:
- 46:15988dc41923
- Child:
- 50:dadad0567349
File content as of revision 47:2467de40951f:
#ifndef __solaESKF_HPP__ #define __solaESKF_HPP__ #include "Matrix.h" #define M_PI 3.141592f class solaESKF { private: int nState; Matrix pihat; Matrix vihat; Matrix qhat; Matrix accBias; Matrix gyroBias; Matrix gravity; 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 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 setQVelocity(float val); void setQAngleError(float val); void setQAccBias(float val); void setQGyroBias(float val); void updateNominal(Matrix acc,Matrix gyro, float att_dt); void updateErrState(Matrix acc,Matrix gyro, float att_dt); void updateGPSVelocity(Matrix velgps,Matrix R); void updateGPSPosition(Matrix posgps,Matrix R); void updateGPS(Matrix posgps,Matrix velgps,Matrix R); void updateAccConstraints(Matrix acc,Matrix R); void updateGyroConstraints(Matrix gyro,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); Matrix computeAngles(); Matrix quatmultiply(Matrix p, Matrix q); }; #endif