Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
- Committer:
- NaotoMorita
- Date:
- 2021-11-10
- Revision:
- 56:c10f1168bd4a
- Parent:
- 55:21611d4cf7e8
- Child:
- 57:e4a38231cfb4
File content as of revision 56:c10f1168bd4a:
#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 magField; 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 getMagField(); 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 setPhatMagField(float val); void setQVelocity(float val); void setQAngleError(float val); void setQAccBias(float val); void setQGyroBias(float val); void setQMagField(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 fuseErr2Nominal(); void computeDcm(Matrix& dcm, Matrix quat); void setQhat(float ex,float ey,float ez); void setGravity(float gx,float gy,float gz); void setMagField(float mx,float my,float mz); void setPihat(float pi_x, float pi_y, float pi_z); 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);