Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
solaESKF.hpp
00001 #ifndef __solaESKF_HPP__ 00002 #define __solaESKF_HPP__ 00003 00004 #include <Eigen/Dense.h> 00005 //#include <Eigen/Core.h> 00006 //#include <Eigen/LU.h> 00007 #include <cmath> 00008 00009 using namespace Eigen; 00010 00011 #ifndef M_PI_F 00012 #define M_PI_F 3.141592f 00013 #endif 00014 00015 class solaESKF 00016 { 00017 private: 00018 00019 int nState; 00020 Vector3f pihat; 00021 Vector3f vihat; 00022 Vector4f qhat; 00023 Vector3f accBias; 00024 Vector3f gyroBias; 00025 Vector3f gravity; 00026 00027 VectorXf errState; 00028 MatrixXf Phat; 00029 MatrixXf Q; 00030 00031 void setDiag(Matrix3f& mat, float val); 00032 void setDiag(MatrixXf& mat, float val); 00033 void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex); 00034 00035 public: 00036 00037 solaESKF(); 00038 00039 Vector3f getPihat(); 00040 Vector3f getVihat(); 00041 Vector4f getQhat(); 00042 Vector3f getAccBias(); 00043 Vector3f getGyroBias(); 00044 Vector3f getGravity(); 00045 VectorXf getErrState(); 00046 VectorXf getState(); 00047 VectorXf getVariance(); 00048 00049 void setPhatPosition(float valNE,float valD); 00050 void setPhatVelocity(float valNE,float valD); 00051 void setPhatAngleError(float val); 00052 void setPhatAccBias(float val); 00053 void setPhatGyroBias(float val); 00054 void setPhatGravity(float val); 00055 void setQVelocity(float valNE,float valD); 00056 void setQAngleError(float val); 00057 void setQAccBias(float val); 00058 void setQGyroBias(float val); 00059 00060 void updateNominal(Vector3f acc, Vector3f gyro, float att_dt); 00061 void updateErrState(Vector3f acc, Vector3f gyro, float att_dt); 00062 void updateGPS(Vector3f posgps,float palt, Vector3f velgps, MatrixXf R); 00063 void updateGPSPosition(Vector3f posgps, float palt, Matrix3f R); 00064 void updateGPSVelocity(Vector3f velgps, Matrix3f R); 00065 void updateAcc(Vector3f acc, Matrix3f R); 00066 void updateHeading(float a, Matrix<float, 1, 1> R); 00067 void updateIMU(float palt,Vector3f acc,float heading, Matrix<float, 5, 5> R); 00068 void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R); 00069 void fuseErr2Nominal(); 00070 void fuseCenter2Nominal(VectorXf errVal); 00071 00072 void computeDcm(Matrix3f& dcm, Vector4f quat); 00073 void setQhat(float ex,float ey,float ez); 00074 void setGravity(float gx,float gy,float gz); 00075 Vector3f calcDynAcc(Vector3f acc); 00076 Vector3f vector2Body(Vector3f veci); 00077 Vector3f vector2NED(Vector3f vecb); 00078 00079 void setPihat(float pi_x, float pi_y,float pi_z); 00080 00081 Vector3f computeAngles(); 00082 Vector4f quatmultiply(Vector4f p, Vector4f q); 00083 00084 static Matrix3f Matrixcross(Vector3f v); 00085 }; 00086 00087 #endif 00088 00089 //void updateGPSVelocity(Matrix velgps,Matrix R); 00090 //void updateGPSPosition(Matrix posgps,Matrix R); 00091 //void updateGyroConstraints(Matrix gyro,Matrix R); 00092 //void updateMag(Matrix mag,Matrix R); 00093 //void updateAccConstraints(Matrix acc,float palt,Matrix R); 00094 //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R); 00095 //void updateGPSPosition(Matrix posgps,float palt,Matrix R); 00096 //
Generated on Wed Jan 11 2023 04:11:21 by 1.7.2