Diff: solaESKF.hpp

Revision:

56:c10f1168bd4a

Parent:

55:21611d4cf7e8

Child:

57:e4a38231cfb4

--- a/solaESKF.hpp	Mon Nov 08 09:19:55 2021 +0000
+++ b/solaESKF.hpp	Wed Nov 10 05:21:36 2021 +0000
@@ -7,6 +7,7 @@
 
 #define M_PI 3.141592f
 
+extern Serial twelite;
 
 class solaESKF
 {
@@ -19,6 +20,7 @@
     Matrix accBias;
     Matrix gyroBias;
     Matrix gravity;
+    Matrix magField;
     
     Matrix errState;
     Matrix Phat;
@@ -37,6 +39,7 @@
     Matrix getAccBias();
     Matrix getGyroBias();
     Matrix getGravity();
+    Matrix getMagField();
     Matrix getErrState();
     
     void setPhatPosition(float val);
@@ -45,24 +48,23 @@
     void setPhatAccBias(float val);
     void setPhatGyroBias(float val);
     void setPhatGravity(float val);
+    void setPhatMagField(float val);
     void setQVelocity(float val);
     void setQAngleError(float val);
     void setQAccBias(float val);
     void setQGyroBias(float val);
+    void setQMagField(float val);
     
     void updateNominal(Matrix acc,Matrix gyro, float att_dt);
     void updateErrState(Matrix acc,Matrix gyro, float att_dt);
-    void updateGPSVelocity(Matrix velgps,Matrix R);
-    void updateGPSPosition(Matrix posgps,Matrix R);
-    void updateGPS(Matrix posgps,Matrix velgps,Matrix R);
-    void updateAccConstraints(Matrix acc,float palt,Matrix R);
-    void updateGyroConstraints(Matrix gyro,Matrix R);
-    void updateMag(Matrix mag,Matrix R);
+    void updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R);
+    void updateImuConstraints(Matrix acc,Matrix mag,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, float pi_z);
     
@@ -71,4 +73,10 @@
     
 };
 
-#endif
\ No newline at end of file
+#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);
\ No newline at end of file