Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
- Committer:
- cocorlow
- Date:
- 2021-12-06
- Revision:
- 78:e36a7b844fb5
- Parent:
- 76:cd1b21936ef1
- Child:
- 79:365ea9277167
File content as of revision 78:e36a7b844fb5:
#ifndef __solaESKF_HPP__ #define __solaESKF_HPP__ #include <Eigen/Core.h> #include <Eigen/LU.h> #include <cmath> using namespace Eigen; #ifndef M_PI_F #define M_PI_F 3.141592f #endif class solaESKF { private: int nState; Vector3f pihat; Vector3f vihat; Vector4f qhat; Vector3f accBias; Vector3f gyroBias; Vector3f gravity; VectorXf errState; MatrixXf Phat; MatrixXf Q; void setDiag(MatrixXf& mat, float val); void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex); public: solaESKF(); Vector3f getPihat(); Vector3f getVihat(); Vector4f getQhat(); Vector3f getAccBias(); Vector3f getGyroBias(); Vector3f getGravity(); VectorXf 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(Vector3f acc, Vector3f gyro, float att_dt); void updateErrState(Vector3f acc, Vector3f gyro, float att_dt); void updateGPS(Vector3f posgps,float palt, Vector3f velgps, MatrixXf R); void updateGPSPosition(Vector3f posgps, float palt, Matrix3f R); void updateGPSVelocity(Vector3f velgps, Matrix3f R); void updateAcc(Vector3f acc, Matrix3f R); void updateHeading(float a, Matrix3f R); void fuseErr2Nominal(); void computeDcm(Matrix3f& dcm, Vector4f quat); void setQhat(float ex,float ey,float ez); void setGravity(float gx,float gy,float gz); Vector3f calcDynAcc(Vector3f acc); void setPihat(float pi_x, float pi_y); Vector3f computeAngles(); Vector4f quatmultiply(Vector4f p, Vector4f q); static Matrix3f Matrixcross(Vector3f v); }; #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); //