Diff: solaESKF.hpp

Revision:

44:7d82e63b6a86

Child:

45:df4618814803

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/solaESKF.hpp	Tue Oct 26 05:33:11 2021 +0000
@@ -0,0 +1,46 @@
+#ifndef __solaESKF_HPP__
+#define __solaESKF_HPP__
+
+#include "Matrix.h"
+
+#define M_PI 3.141592f
+
+
+class solaESKF
+{
+private:
+
+    int nState;
+        
+public:
+    
+    Matrix pihat;
+    Matrix vihat;
+    Matrix qhat;
+    Matrix accBias;
+    Matrix gyroBias;
+    Matrix gravity;
+    
+    Matrix errState;
+    Matrix Phat;
+    Matrix Q;
+    
+    solaESKF();
+    
+    void setDiag(Matrix& mat, float val);
+    void setBlockDiag(Matrix& mat, float val,int startIndex, int endIndez);
+    void updateNominal(Matrix acc,Matrix gyro, float att_dt);
+    void updateErrState(Matrix acc,Matrix gyro, float att_dt);
+    
+    void fuseErr2Nominal();
+    
+    void computeDcm(Matrix& dcm, Matrix quat);
+    void setQhat(float ex,float ey,float ez);
+    void setGravity(float gx,float gy,float gz);
+    
+    Matrix computeAngles();
+    Matrix quatmultiply(Matrix p, Matrix q);
+    
+};
+
+#endif
\ No newline at end of file