Diff: solaESKF.hpp

Revision:

62:5519d34eb6e8

Parent:

61:5e5c4fe12440

Child:

65:c25d7810de44

--- a/solaESKF.hpp	Tue Nov 16 13:56:44 2021 +0000
+++ b/solaESKF.hpp	Tue Nov 16 14:17:40 2021 +0000
@@ -20,37 +20,45 @@
     Matrix accBias;
     Matrix gyroBias;
     Matrix gravity;
-    
-    
+    Matrix magBias;
+    float magRadius;
+
+
     Matrix errState;
     Matrix Phat;
     Matrix Q;
-    
+
     void setDiag(Matrix& mat, float val);
     void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex);
-        
+
 public:
-    
+
     solaESKF();
-    
+
     Matrix getPihat();
     Matrix getVihat();
     Matrix getQhat();
     Matrix getAccBias();
     Matrix getGyroBias();
     Matrix getGravity();
+    Matrix getMagBias();
+    float getMagRadius();
     Matrix getErrState();
-    
+
     void setPhatPosition(float val);
     void setPhatVelocity(float val);
     void setPhatAngleError(float val);
     void setPhatAccBias(float val);
     void setPhatGyroBias(float val);
     void setPhatGravity(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 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);