Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
- Committer:
- NaotoMorita
- Date:
- 20 months ago
- Revision:
- 87:6862abea60ed
- Parent:
- 86:cc2c0025d820
File content as of revision 87:6862abea60ed:
#ifndef __solaESKF_HPP__ #define __solaESKF_HPP__ #include <Eigen/Dense.h> //#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(Matrix3f& mat, float val); 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(); VectorXf getState(); VectorXf getVariance(); void setPhatPosition(float valNE,float valD); void setPhatVelocity(float valNE,float valD); void setPhatAngleError(float val); void setPhatAccBias(float val); void setPhatGyroBias(float val); void setPhatGravity(float val); void setQVelocity(float valNE,float valD); 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, Matrix<float, 1, 1> R); void updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R); void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R); void fuseErr2Nominal(); void fuseCenter2Nominal(VectorXf errVal); 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); Vector3f vector2Body(Vector3f veci); Vector3f vector2NED(Vector3f vecb); void setPihat(float pi_x, float pi_y,float pi_z); 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); //