HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

Diff: solaESKF.cpp

Revision:
86:cc2c0025d820
Parent:
84:fb14acabefd3
Child:
88:06d5b2cc5e20
diff -r fb14acabefd3 -r cc2c0025d820 solaESKF.cpp
--- a/solaESKF.cpp	Wed Jul 13 05:08:01 2022 +0000
+++ b/solaESKF.cpp	Tue Dec 20 06:24:27 2022 +0000
@@ -652,16 +652,24 @@
     return dynAcc;
 }
 
-Vector3f solaESKF::calcVb()
+Vector3f solaESKF::vector2Body(Vector3f veci)
 {
     Matrix3f dcm;
     computeDcm(dcm, qhat);
     Matrix3f tdcm = dcm.transpose();
 
-    Vector3f Vb = tdcm*vihat;
-    return Vb;
+    return tdcm*veci ;
 }
 
+Vector3f solaESKF::vector2NED(Vector3f vecb)
+{
+    Matrix3f dcm;
+    computeDcm(dcm, qhat);
+
+    return dcm*vecb;
+}
+
+
 
 void solaESKF::setGravity(float gx,float gy,float gz)
 {