Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

Revision:
59:03fe5e16a33c
Parent:
58:93ba28cf5cb3
Child:
60:f4b4231a8d3f
--- a/solaESKF.hpp	Fri Nov 12 09:03:10 2021 +0000
+++ b/solaESKF.hpp	Mon Nov 15 13:41:40 2021 +0000
@@ -20,7 +20,9 @@
     Matrix accBias;
     Matrix gyroBias;
     Matrix gravity;
-    Matrix magField;
+    Matrix magBias;
+    float magRadius;
+    
     
     Matrix errState;
     Matrix Phat;
@@ -39,7 +41,8 @@
     Matrix getAccBias();
     Matrix getGyroBias();
     Matrix getGravity();
-    Matrix getMagField();
+    Matrix getMagBias();
+    float getMagRadius();
     Matrix getErrState();
     
     void setPhatPosition(float val);
@@ -48,26 +51,24 @@
     void setPhatAccBias(float val);
     void setPhatGyroBias(float val);
     void setPhatGravity(float val);
-    void setPhatMagField(float val);
+    void setPhatMagBias(float val);
+    void setPhatMagRadius(float val);
     void setQVelocity(float val);
     void setQAngleError(float val);
     void setQAccBias(float val);
     void setQGyroBias(float val);
-    void setQMagField(float val);
+    void setQMagBias(float val);
+    void setQMagRadius(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 mag,Matrix R);
-    void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R);
-    void updateGPSPosition(Matrix posgps,float palt,Matrix R);
+    void updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R);
     void updateImuConstraints(Matrix acc,Matrix mag,Matrix R);
-    void updateMag(Matrix mag,float palt,Matrix R);
     void fuseErr2Nominal();
     
     void computeDcm(Matrix& dcm, Matrix quat);
     void setQhat(float ex,float ey,float ez);
     void setGravity(float gx,float gy,float gz);
-    void setMagField(float mx,float my,float mz);
     
     void setPihat(float pi_x, float pi_y);
     
@@ -82,4 +83,7 @@
     //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);
\ No newline at end of file
+    //void updateAccConstraints(Matrix acc,float palt,Matrix R);
+    //void updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R);
+    //void updateGPSPosition(Matrix posgps,float palt,Matrix R);
+    //    void updateMag(Matrix mag,float palt,Matrix R);
\ No newline at end of file