Diff: solaESKF.hpp

Revision:

80:b241c058df83

Parent:

79:365ea9277167

Child:

81:230a3d2b0493

diff -r 365ea9277167 -r b241c058df83 solaESKF.hpp
--- a/solaESKF.hpp	Fri Dec 10 10:42:09 2021 +0000
+++ b/solaESKF.hpp	Fri Jun 24 05:43:46 2022 +0000
@@ -28,6 +28,7 @@
     MatrixXf Phat;
     MatrixXf Q;
 
+    void setDiag(Matrix3f& mat, float val);
     void setDiag(MatrixXf& mat, float val);
     void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex);
 
@@ -42,14 +43,16 @@
     Vector3f getGyroBias();
     Vector3f getGravity();
     VectorXf getErrState();
+    VectorXf getState();
+    VectorXf getVariance();
 
-    void setPhatPosition(float val);
-    void setPhatVelocity(float val);
+    void setPhatPosition(float valNE,float valD);
+    void setPhatVelocity(float valNE,float valD);
     void setPhatAngleError(float val);
     void setPhatAccBias(float val);
     void setPhatGyroBias(float val);
     void setPhatGravity(float val);
-    void setQVelocity(float val);
+    void setQVelocity(float valNE,float valD);
     void setQAngleError(float val);
     void setQAccBias(float val);
     void setQGyroBias(float val);
@@ -61,6 +64,8 @@
     void updateGPSVelocity(Vector3f velgps, Matrix3f R);
     void updateAcc(Vector3f acc, Matrix3f R); 
     void updateHeading(float a, Matrix<float, 1, 1> R);  
+    void updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R);
+    void updateWhole(Vector3f posgps,float palt, Vector3f velgps, Vector3f acc,float heading, MatrixXf R);
     void fuseErr2Nominal();
     
     void computeDcm(Matrix3f& dcm, Vector4f quat);