Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.hpp
- Revision:
- 78:e36a7b844fb5
- Parent:
- 76:cd1b21936ef1
- Child:
- 79:365ea9277167
diff -r 780ce6556041 -r e36a7b844fb5 solaESKF.hpp --- a/solaESKF.hpp Tue Nov 30 11:26:56 2021 +0000 +++ b/solaESKF.hpp Mon Dec 06 08:24:13 2021 +0000 @@ -1,44 +1,46 @@ #ifndef __solaESKF_HPP__ #define __solaESKF_HPP__ -#include "Matrix.h" -#include "MatrixMath.h" +#include <Eigen/Core.h> +#include <Eigen/LU.h> #include <cmath> -#define M_PI 3.141592f +using namespace Eigen; + +#ifndef M_PI_F +#define M_PI_F 3.141592f +#endif class solaESKF { private: int nState; - Matrix pihat; - Matrix vihat; - Matrix qhat; - Matrix accBias; - Matrix gyroBias; - Matrix gravity; - + Vector3f pihat; + Vector3f vihat; + Vector4f qhat; + Vector3f accBias; + Vector3f gyroBias; + Vector3f gravity; + VectorXf errState; + MatrixXf Phat; + MatrixXf Q; - Matrix errState; - Matrix Phat; - Matrix Q; - - void setDiag(Matrix& mat, float val); - void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex); + void setDiag(MatrixXf& mat, float val); + void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex); public: solaESKF(); - Matrix getPihat(); - Matrix getVihat(); - Matrix getQhat(); - Matrix getAccBias(); - Matrix getGyroBias(); - Matrix getGravity(); - Matrix getErrState(); + Vector3f getPihat(); + Vector3f getVihat(); + Vector4f getQhat(); + Vector3f getAccBias(); + Vector3f getGyroBias(); + Vector3f getGravity(); + VectorXf getErrState(); void setPhatPosition(float val); void setPhatVelocity(float val); @@ -51,25 +53,26 @@ 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 updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R); - void updateGPSPosition(Matrix posgps,float palt,Matrix R); - void updateGPSVelocity(Matrix velgps,Matrix R); - void updateAcc(Matrix acc,Matrix R); - void updateHeading(float a,Matrix R); + 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(Matrix& dcm, Matrix quat); + void computeDcm(Matrix3f& dcm, Vector4f quat); void setQhat(float ex,float ey,float ez); void setGravity(float gx,float gy,float gz); - Matrix calcDynAcc(Matrix acc); + Vector3f calcDynAcc(Vector3f acc); void setPihat(float pi_x, float pi_y); - Matrix computeAngles(); - Matrix quatmultiply(Matrix p, Matrix q); + Vector3f computeAngles(); + Vector4f quatmultiply(Vector4f p, Vector4f q); + static Matrix3f Matrixcross(Vector3f v); }; #endif