fix coordinate transformation (C++)

istellartech · May 27, 2024 · e5426c1 · e5426c1
1 parent 4ad0995
commit e5426c1
Showing 1 changed file with 2 additions and 4 deletions.
diff --git a/src/wrapper_coordinate.hpp b/src/wrapper_coordinate.hpp
@@ -144,10 +144,8 @@ vec4d quat_from_euler(double az, double el, double ro) {
 vec3d gravity(vec3d pos) { return gravityECI(pos); }
 
 vec4d quat_nedg2body(vec4d quat_eci2body, vec3d pos_eci, double t) {
-  Eigen::Quaterniond q(quat_eci2body[0], quat_eci2body[1], quat_eci2body[2],
-                       quat_eci2body[3]);
-  Eigen::Quaterniond q_out = Coordinate::quat_ned2body(q, pos_eci, t);
-  return vec4d(q_out.w(), q_out.x(), q_out.y(), q_out.z());
+  vec4d q = quat_eci2nedg(pos_eci, t);
+  return quatmult(conj(q), quat_eci2body);
 }
 
 vec3d euler_from_quat(vec4d q) {