HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

Diff: solaESKF.cpp

Revision:
82:662b810155e9
Parent:
81:230a3d2b0493
Child:
83:8f6ae61d47ac
--- a/solaESKF.cpp	Fri Jun 24 09:10:18 2022 +0000
+++ b/solaESKF.cpp	Wed Jun 29 01:39:26 2022 +0000
@@ -613,6 +613,17 @@
     return dynAcc;
 }
 
+Vector3f solaESKF::calcDynAcc(Vector3f acc)
+{
+    Vector3f accm = acc - accBias;
+    Matrix3f dcm;
+    computeDcm(dcm, qhat);
+    Matrix3f tdcm = dcm.transpose();
+
+    Vector3f Vb = tdcm*vihat;
+    return dynAcc;
+}
+
 
 void solaESKF::setGravity(float gx,float gy,float gz)
 {
@@ -758,7 +769,7 @@
 {
     pihat(0) = pi_x;
     pihat(1) = pi_y;
-    pihat(3,1) = pi_z;
+    pihat(2) = pi_z;
 }
 
 Matrix3f solaESKF::Matrixcross(Vector3f v)