Diff: solaESKF.hpp

Revision:

78:e36a7b844fb5

Parent:

76:cd1b21936ef1

Child:

79:365ea9277167

--- 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